Add arm code to 2023
Signed-off-by: Maxwell Henderson <mxwhenderson@gmail.com>
Change-Id: If97b4dd4c8fb16371e21ac6e9667a2d52b1b53a6
diff --git a/y2023/control_loops/superstructure/arm/BUILD b/y2023/control_loops/superstructure/arm/BUILD
new file mode 100644
index 0000000..dc52f02
--- /dev/null
+++ b/y2023/control_loops/superstructure/arm/BUILD
@@ -0,0 +1,64 @@
+cc_library(
+ name = "arm",
+ srcs = [
+ "arm.cc",
+ ],
+ hdrs = [
+ "arm.h",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = [
+ ":generated_graph",
+ "//frc971/control_loops/double_jointed_arm:demo_path",
+ "//frc971/control_loops/double_jointed_arm:ekf",
+ "//frc971/control_loops/double_jointed_arm:graph",
+ "//frc971/control_loops/double_jointed_arm:trajectory",
+ "//frc971/zeroing",
+ "//y2023:constants",
+ "//y2023/control_loops/superstructure:superstructure_position_fbs",
+ "//y2023/control_loops/superstructure:superstructure_status_fbs",
+ "//y2023/control_loops/superstructure/arm:arm_constants",
+ ],
+)
+
+genrule(
+ name = "generated_graph_genrule",
+ outs = [
+ "generated_graph.h",
+ "generated_graph.cc",
+ ],
+ cmd = "$(location //y2023/control_loops/python:graph_codegen) $(OUTS)",
+ target_compatible_with = ["@platforms//os:linux"],
+ tools = [
+ "//y2023/control_loops/python:graph_codegen",
+ ],
+)
+
+cc_library(
+ name = "generated_graph",
+ srcs = [
+ "generated_graph.cc",
+ ],
+ hdrs = ["generated_graph.h"],
+ copts = [
+ "-O1",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = [
+ ":arm_constants",
+ "//frc971/control_loops/double_jointed_arm:graph",
+ "//frc971/control_loops/double_jointed_arm:trajectory",
+ ],
+)
+
+cc_library(
+ name = "arm_constants",
+ hdrs = ["arm_constants.h"],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//frc971/control_loops/double_jointed_arm:dynamics",
+ ],
+)
diff --git a/y2023/control_loops/superstructure/arm/arm.cc b/y2023/control_loops/superstructure/arm/arm.cc
new file mode 100644
index 0000000..73781d2
--- /dev/null
+++ b/y2023/control_loops/superstructure/arm/arm.cc
@@ -0,0 +1,279 @@
+#include "y2023/control_loops/superstructure/arm/arm.h"
+
+namespace y2023 {
+namespace control_loops {
+namespace superstructure {
+namespace arm {
+namespace {
+
+namespace chrono = ::std::chrono;
+using ::aos::monotonic_clock;
+
+constexpr int kMaxBrownoutCount = 4;
+
+} // namespace
+
+Arm::Arm(std::shared_ptr<const constants::Values> values)
+ : values_(values),
+ state_(ArmState::UNINITIALIZED),
+ proximal_zeroing_estimator_(values_->arm_proximal.zeroing),
+ distal_zeroing_estimator_(values_->arm_distal.zeroing),
+ proximal_offset_(0.0),
+ distal_offset_(0.0),
+ max_intake_override_(1000.0),
+ alpha_unitizer_((::Eigen::Matrix<double, 2, 2>() << 1.0 / kAlpha0Max(),
+ 0.0, 0.0, 1.0 / kAlpha1Max())
+ .finished()),
+ dynamics_(kArmConstants),
+ search_graph_(MakeSearchGraph(&dynamics_, &trajectories_, alpha_unitizer_,
+ kVMax())),
+ close_enough_for_full_power_(false),
+ brownout_count_(0),
+ arm_ekf_(&dynamics_),
+ // Go to the start of the first trajectory.
+ follower_(&dynamics_, NeutralPosPoint()),
+ points_(PointList()),
+ current_node_(0) {
+ int i = 0;
+ for (const auto &trajectory : trajectories_) {
+ AOS_LOG(INFO, "trajectory length for edge node %d: %f\n", i,
+ trajectory.trajectory.path().length());
+ ++i;
+ }
+}
+
+void Arm::Reset() { state_ = ArmState::UNINITIALIZED; }
+
+flatbuffers::Offset<superstructure::ArmStatus> Arm::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) {
+ ::Eigen::Matrix<double, 2, 1> Y;
+ const bool outputs_disabled =
+ ((proximal_output == nullptr) || (distal_output == nullptr));
+ if (outputs_disabled) {
+ ++brownout_count_;
+ } else {
+ brownout_count_ = 0;
+ }
+
+ uint32_t filtered_goal = 0;
+ if (unsafe_goal != nullptr) {
+ filtered_goal = *unsafe_goal;
+ }
+
+ Y << position->proximal()->encoder() + proximal_offset_,
+ position->distal()->encoder() + distal_offset_;
+
+ proximal_zeroing_estimator_.UpdateEstimate(*position->proximal());
+ distal_zeroing_estimator_.UpdateEstimate(*position->distal());
+
+ if (proximal_output != nullptr) {
+ *proximal_output = 0.0;
+ }
+ if (distal_output != nullptr) {
+ *distal_output = 0.0;
+ }
+
+ arm_ekf_.Correct(Y, kDt());
+
+ if (::std::abs(arm_ekf_.X_hat(0) - follower_.theta(0)) <= 0.05 &&
+ ::std::abs(arm_ekf_.X_hat(2) - follower_.theta(1)) <= 0.05) {
+ close_enough_for_full_power_ = true;
+ }
+ if (::std::abs(arm_ekf_.X_hat(0) - follower_.theta(0)) >= 1.10 ||
+ ::std::abs(arm_ekf_.X_hat(2) - follower_.theta(1)) >= 1.10) {
+ close_enough_for_full_power_ = false;
+ }
+
+ switch (state_) {
+ case ArmState::UNINITIALIZED:
+ // Wait in the uninitialized state until the intake is initialized.
+ AOS_LOG(DEBUG, "Uninitialized, waiting for intake\n");
+ state_ = ArmState::ZEROING;
+ proximal_zeroing_estimator_.Reset();
+ distal_zeroing_estimator_.Reset();
+ break;
+
+ case ArmState::ZEROING:
+ // Zero by not moving.
+ if (proximal_zeroing_estimator_.zeroed() &&
+ distal_zeroing_estimator_.zeroed()) {
+ state_ = ArmState::DISABLED;
+
+ proximal_offset_ = proximal_zeroing_estimator_.offset();
+ distal_offset_ = distal_zeroing_estimator_.offset();
+
+ Y << position->proximal()->encoder() + proximal_offset_,
+ position->distal()->encoder() + distal_offset_;
+
+ // TODO(austin): Offset ekf rather than reset it. Since we aren't
+ // moving at this point, it's pretty safe to do this.
+ ::Eigen::Matrix<double, 4, 1> X;
+ X << Y(0), 0.0, Y(1), 0.0;
+ arm_ekf_.Reset(X);
+ } else {
+ break;
+ }
+ [[fallthrough]];
+
+ case ArmState::DISABLED: {
+ follower_.SwitchTrajectory(nullptr);
+ close_enough_for_full_power_ = false;
+
+ const ::Eigen::Matrix<double, 2, 1> current_theta =
+ (::Eigen::Matrix<double, 2, 1>() << arm_ekf_.X_hat(0),
+ arm_ekf_.X_hat(2))
+ .finished();
+ uint32_t best_index = 0;
+ double best_distance = (points_[0] - current_theta).norm();
+ uint32_t current_index = 0;
+ for (const ::Eigen::Matrix<double, 2, 1> &point : points_) {
+ const double new_distance = (point - current_theta).norm();
+ if (new_distance < best_distance) {
+ best_distance = new_distance;
+ best_index = current_index;
+ }
+ ++current_index;
+ }
+ follower_.set_theta(points_[best_index]);
+ current_node_ = best_index;
+
+ if (!outputs_disabled) {
+ state_ = ArmState::GOTO_PATH;
+ } else {
+ break;
+ }
+ }
+ [[fallthrough]];
+
+ case ArmState::GOTO_PATH:
+ if (outputs_disabled) {
+ state_ = ArmState::DISABLED;
+ } else if (trajectory_override) {
+ follower_.SwitchTrajectory(nullptr);
+ current_node_ = filtered_goal;
+ follower_.set_theta(points_[current_node_]);
+ state_ = ArmState::GOTO_PATH;
+ } else if (close_enough_for_full_power_) {
+ state_ = ArmState::RUNNING;
+ }
+ break;
+
+ case ArmState::RUNNING:
+ // ESTOP if we hit the hard limits.
+ // TODO(austin): Pick some sane limits.
+ if (proximal_zeroing_estimator_.error() ||
+ distal_zeroing_estimator_.error()) {
+ AOS_LOG(ERROR, "Zeroing error ESTOP\n");
+ state_ = ArmState::ESTOP;
+ } else if (outputs_disabled && brownout_count_ > kMaxBrownoutCount) {
+ state_ = ArmState::DISABLED;
+ } else if (trajectory_override) {
+ follower_.SwitchTrajectory(nullptr);
+ current_node_ = filtered_goal;
+ follower_.set_theta(points_[current_node_]);
+ state_ = ArmState::GOTO_PATH;
+ }
+ break;
+
+ case ArmState::ESTOP:
+ AOS_LOG(ERROR, "Estop\n");
+ break;
+ }
+
+ const bool disable = outputs_disabled || (state_ != ArmState::RUNNING &&
+ state_ != ArmState::GOTO_PATH);
+ if (disable) {
+ close_enough_for_full_power_ = false;
+ }
+
+ if (state_ == ArmState::RUNNING && unsafe_goal != nullptr) {
+ if (current_node_ != filtered_goal) {
+ AOS_LOG(INFO, "Goal is different\n");
+ if (filtered_goal >= search_graph_.num_vertexes()) {
+ AOS_LOG(ERROR, "goal node out of range ESTOP\n");
+ state_ = ArmState::ESTOP;
+ } else if (follower_.path_distance_to_go() > 1e-3) {
+ // Still on the old path segment. Can't change yet.
+ } else {
+ search_graph_.SetGoal(filtered_goal);
+
+ size_t min_edge = 0;
+ double min_cost = ::std::numeric_limits<double>::infinity();
+ for (const SearchGraph::HalfEdge &edge :
+ search_graph_.Neighbors(current_node_)) {
+ const double cost = search_graph_.GetCostToGoal(edge.dest);
+ if (cost < min_cost) {
+ min_edge = edge.edge_id;
+ min_cost = cost;
+ }
+ }
+ // Ok, now we know which edge we are on. Figure out the path and
+ // trajectory.
+ const SearchGraph::Edge &next_edge = search_graph_.edges()[min_edge];
+ AOS_LOG(INFO, "Switching from node %d to %d along edge %d\n",
+ static_cast<int>(current_node_),
+ static_cast<int>(next_edge.end), static_cast<int>(min_edge));
+ vmax_ = trajectories_[min_edge].vmax;
+ follower_.SwitchTrajectory(&trajectories_[min_edge].trajectory);
+ current_node_ = next_edge.end;
+ }
+ }
+ }
+
+ const double max_operating_voltage =
+ close_enough_for_full_power_
+ ? kOperatingVoltage()
+ : (state_ == ArmState::GOTO_PATH ? kGotoPathVMax() : kPathlessVMax());
+ follower_.Update(arm_ekf_.X_hat(), disable, kDt(), vmax_,
+ max_operating_voltage);
+ AOS_LOG(INFO, "Max voltage: %f\n", max_operating_voltage);
+
+ flatbuffers::Offset<frc971::PotAndAbsoluteEncoderEstimatorState>
+ proximal_estimator_state_offset =
+ proximal_zeroing_estimator_.GetEstimatorState(fbb);
+ flatbuffers::Offset<frc971::PotAndAbsoluteEncoderEstimatorState>
+ distal_estimator_state_offset =
+ distal_zeroing_estimator_.GetEstimatorState(fbb);
+
+ superstructure::ArmStatus::Builder status_builder(*fbb);
+ status_builder.add_proximal_estimator_state(proximal_estimator_state_offset);
+ status_builder.add_distal_estimator_state(distal_estimator_state_offset);
+
+ status_builder.add_goal_theta0(follower_.theta(0));
+ status_builder.add_goal_theta1(follower_.theta(1));
+ status_builder.add_goal_omega0(follower_.omega(0));
+ status_builder.add_goal_omega1(follower_.omega(1));
+
+ status_builder.add_theta0(arm_ekf_.X_hat(0));
+ status_builder.add_theta1(arm_ekf_.X_hat(2));
+ status_builder.add_omega0(arm_ekf_.X_hat(1));
+ status_builder.add_omega1(arm_ekf_.X_hat(3));
+ status_builder.add_voltage_error0(arm_ekf_.X_hat(4));
+ status_builder.add_voltage_error1(arm_ekf_.X_hat(5));
+
+ if (!disable) {
+ *proximal_output = ::std::max(
+ -kOperatingVoltage(), ::std::min(kOperatingVoltage(), follower_.U(0)));
+ *distal_output = ::std::max(
+ -kOperatingVoltage(), ::std::min(kOperatingVoltage(), follower_.U(1)));
+ }
+
+ status_builder.add_path_distance_to_go(follower_.path_distance_to_go());
+ status_builder.add_current_node(current_node_);
+
+ status_builder.add_zeroed(zeroed());
+ status_builder.add_estopped(estopped());
+ status_builder.add_state(state_);
+ status_builder.add_failed_solutions(follower_.failed_solutions());
+
+ arm_ekf_.Predict(follower_.U(), kDt());
+ return status_builder.Finish();
+}
+
+} // namespace arm
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2023
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_
diff --git a/y2023/control_loops/superstructure/arm/arm_constants.h b/y2023/control_loops/superstructure/arm/arm_constants.h
new file mode 100644
index 0000000..5611eaa
--- /dev/null
+++ b/y2023/control_loops/superstructure/arm/arm_constants.h
@@ -0,0 +1,53 @@
+#ifndef Y2023_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_ARM_CONSTANTS_H_
+#define Y2023_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_ARM_CONSTANTS_H_
+
+#include "frc971/control_loops/double_jointed_arm/dynamics.h"
+
+namespace y2023 {
+namespace control_loops {
+namespace superstructure {
+namespace arm {
+
+constexpr double kEfficiencyTweak = 0.95;
+constexpr double kStallTorque = 4.69 * kEfficiencyTweak;
+constexpr double kFreeSpeed = (6380.0 / 60.0) * 2.0 * M_PI;
+constexpr double kStallCurrent = 257.0;
+
+constexpr ::frc971::control_loops::arm::ArmConstants kArmConstants = {
+ .l1 = 20 * 0.0254,
+ .l2 = 38 * 0.0254,
+
+ .m1 = 9.34 / 2.2,
+ .m2 = 9.77 / 2.2,
+
+ // Moment of inertia of the joints in kg m^2
+ .j1 = 2957.05 * 0.0002932545454545454,
+ .j2 = 2824.70 * 0.0002932545454545454,
+
+ // Radius of the center of mass of the joints in meters.
+ .r1 = 21.64 * 0.0254,
+ .r2 = 26.70 * 0.0254,
+
+ // Gear ratios for the two joints.
+ .g1 = 55.0,
+ .g2 = 106.0,
+
+ // Falcon motor constants.
+ .efficiency_tweak = kEfficiencyTweak,
+ .stall_torque = kStallTorque,
+ .free_speed = kFreeSpeed,
+ .stall_current = kStallCurrent,
+ .resistance = 12.0 / kStallCurrent,
+ .Kv = kFreeSpeed / 12.0,
+ .Kt = kStallTorque / kStallCurrent,
+
+ // Number of motors on the distal joint.
+ .num_distal_motors = 1.0,
+};
+
+} // namespace arm
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2023
+
+#endif // Y2023_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_ARM_CONSTANTS_H_