Add arm code to 2023
Signed-off-by: Maxwell Henderson <mxwhenderson@gmail.com>
Change-Id: If97b4dd4c8fb16371e21ac6e9667a2d52b1b53a6
diff --git a/y2023/control_loops/superstructure/arm/arm.h b/y2023/control_loops/superstructure/arm/arm.h
new file mode 100644
index 0000000..a595ace
--- /dev/null
+++ b/y2023/control_loops/superstructure/arm/arm.h
@@ -0,0 +1,114 @@
+#ifndef Y2023_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_ARM_H_
+#define Y2023_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_ARM_H_
+
+#include "aos/time/time.h"
+#include "frc971/control_loops/double_jointed_arm/dynamics.h"
+#include "frc971/control_loops/double_jointed_arm/ekf.h"
+#include "frc971/control_loops/double_jointed_arm/graph.h"
+#include "frc971/control_loops/double_jointed_arm/trajectory.h"
+#include "frc971/zeroing/zeroing.h"
+#include "y2023/constants.h"
+#include "y2023/control_loops/superstructure/arm/generated_graph.h"
+#include "y2023/control_loops/superstructure/superstructure_position_generated.h"
+#include "y2023/control_loops/superstructure/superstructure_status_generated.h"
+
+using frc971::control_loops::arm::EKF;
+using frc971::control_loops::arm::TrajectoryFollower;
+
+namespace y2023 {
+namespace control_loops {
+namespace superstructure {
+namespace arm {
+
+class Arm {
+ public:
+ Arm(std::shared_ptr<const constants::Values> values);
+
+ // if true, tune down all the constants for testing.
+ static constexpr bool kGrannyMode() { return false; }
+ // the operating voltage.
+ static constexpr double kOperatingVoltage() {
+ return kGrannyMode() ? 5.0 : 12.0;
+ }
+
+ static constexpr double kDt() { return 0.00505; }
+
+ static constexpr double kAlpha0Max() { return kGrannyMode() ? 5.0 : 15.0; }
+ static constexpr double kAlpha1Max() { return kGrannyMode() ? 5.0 : 15.0; }
+
+ static constexpr double kVMax() { return kGrannyMode() ? 5.0 : 11.5; }
+
+ static constexpr double kPathlessVMax() { return 5.0; }
+ static constexpr double kGotoPathVMax() { return 6.0; }
+
+ flatbuffers::Offset<superstructure::ArmStatus> Iterate(
+ const ::aos::monotonic_clock::time_point /*monotonic_now*/,
+ const uint32_t *unsafe_goal, const superstructure::ArmPosition *position,
+ bool trajectory_override, double *proximal_output, double *distal_output,
+ bool /*intake*/, bool /*spit*/, flatbuffers::FlatBufferBuilder *fbb);
+
+ void Reset();
+
+ ArmState state() const { return state_; }
+ bool estopped() const { return state_ == ArmState::ESTOP; }
+
+ bool zeroed() const {
+ return (proximal_zeroing_estimator_.zeroed() &&
+ distal_zeroing_estimator_.zeroed());
+ }
+ // Returns the maximum position for the intake. This is used to override the
+ // intake position to release the box when the state machine is lifting.
+ double max_intake_override() const { return max_intake_override_; }
+ uint32_t current_node() const { return current_node_; }
+ double path_distance_to_go() { return follower_.path_distance_to_go(); }
+
+ private:
+ bool AtState(uint32_t state) const { return current_node_ == state; }
+ bool NearEnd(double threshold = 0.03) const {
+ return ::std::abs(arm_ekf_.X_hat(0) - follower_.theta(0)) <= threshold &&
+ ::std::abs(arm_ekf_.X_hat(2) - follower_.theta(1)) <= threshold &&
+ follower_.path_distance_to_go() < 1e-3;
+ }
+
+ std::shared_ptr<const constants::Values> values_;
+ ArmState state_;
+
+ ::frc971::zeroing::PotAndAbsoluteEncoderZeroingEstimator
+ proximal_zeroing_estimator_;
+ ::frc971::zeroing::PotAndAbsoluteEncoderZeroingEstimator
+ distal_zeroing_estimator_;
+
+ double proximal_offset_;
+ double distal_offset_;
+ double max_intake_override_;
+
+ const ::Eigen::Matrix<double, 2, 2> alpha_unitizer_;
+ double vmax_ = kVMax();
+
+ frc971::control_loops::arm::Dynamics dynamics_;
+
+ ::std::vector<TrajectoryAndParams> trajectories_;
+
+ SearchGraph search_graph_;
+
+ bool close_enough_for_full_power_;
+
+ size_t brownout_count_;
+
+ EKF arm_ekf_;
+
+ TrajectoryFollower follower_;
+
+ const ::std::vector<::Eigen::Matrix<double, 2, 1>> points_;
+
+ // Start at the 0th index.
+ uint32_t current_node_;
+
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+};
+} // namespace arm
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2023
+
+#endif // Y2023_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_ARM_H_