Add arm code to 2023
Signed-off-by: Maxwell Henderson <mxwhenderson@gmail.com>
Change-Id: If97b4dd4c8fb16371e21ac6e9667a2d52b1b53a6
diff --git a/y2023/BUILD b/y2023/BUILD
index 32965ae..d5f85ca 100644
--- a/y2023/BUILD
+++ b/y2023/BUILD
@@ -219,6 +219,7 @@
"//frc971/control_loops:static_zeroing_single_dof_profiled_subsystem",
"//frc971/shooter_interpolation:interpolation",
"//y2023/control_loops/drivetrain:polydrivetrain_plants",
+ "//y2023/control_loops/superstructure/arm:arm_constants",
"@com_github_google_glog//:glog",
"@com_google_absl//absl/base",
],
diff --git a/y2023/constants.cc b/y2023/constants.cc
index 843af2b..89d9f49 100644
--- a/y2023/constants.cc
+++ b/y2023/constants.cc
@@ -15,25 +15,60 @@
namespace y2023 {
namespace constants {
-const int Values::kZeroingSampleSize;
-
Values MakeValues(uint16_t team) {
LOG(INFO) << "creating a Constants for team: " << team;
Values r;
+ auto *const arm_proximal = &r.arm_proximal;
+ auto *const arm_distal = &r.arm_distal;
+
+ arm_proximal->zeroing.average_filter_size = Values::kZeroingSampleSize;
+ arm_proximal->zeroing.one_revolution_distance =
+ M_PI * 2.0 * constants::Values::kProximalEncoderRatio();
+ arm_proximal->zeroing.zeroing_threshold = 0.0005;
+ arm_proximal->zeroing.moving_buffer_size = 20;
+ arm_proximal->zeroing.allowable_encoder_error = 0.9;
+
+ arm_distal->zeroing.average_filter_size = Values::kZeroingSampleSize;
+ arm_distal->zeroing.one_revolution_distance =
+ M_PI * 2.0 * constants::Values::kDistalEncoderRatio();
+ arm_distal->zeroing.zeroing_threshold = 0.0005;
+ arm_distal->zeroing.moving_buffer_size = 20;
+ arm_distal->zeroing.allowable_encoder_error = 0.9;
+
switch (team) {
// A set of constants for tests.
case 1:
+ arm_proximal->zeroing.measured_absolute_position = 0.0;
+ arm_proximal->potentiometer_offset = 0.0;
+
+ arm_distal->zeroing.measured_absolute_position = 0.0;
+ arm_distal->potentiometer_offset = 0.0;
break;
case kCompTeamNumber:
+ arm_proximal->zeroing.measured_absolute_position = 0.0;
+ arm_proximal->potentiometer_offset = 0.0;
+
+ arm_distal->zeroing.measured_absolute_position = 0.0;
+ arm_distal->potentiometer_offset = 0.0;
break;
case kPracticeTeamNumber:
+ arm_proximal->zeroing.measured_absolute_position = 0.0;
+ arm_proximal->potentiometer_offset = 0.0;
+
+ arm_distal->zeroing.measured_absolute_position = 0.0;
+ arm_distal->potentiometer_offset = 0.0;
break;
case kCodingRobotTeamNumber:
+ arm_proximal->zeroing.measured_absolute_position = 0.0;
+ arm_proximal->potentiometer_offset = 0.0;
+
+ arm_distal->zeroing.measured_absolute_position = 0.0;
+ arm_distal->potentiometer_offset = 0.0;
break;
default:
diff --git a/y2023/constants.h b/y2023/constants.h
index edcb794..64153af 100644
--- a/y2023/constants.h
+++ b/y2023/constants.h
@@ -9,6 +9,7 @@
#include "frc971/control_loops/pose.h"
#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h"
#include "y2023/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
+#include "y2023/control_loops/superstructure/arm/arm_constants.h"
namespace y2023 {
namespace constants {
@@ -51,6 +52,29 @@
control_loops::drivetrain::kHighOutputRatio *
control_loops::drivetrain::kWheelRadius;
}
+ static constexpr double kProximalEncoderCountsPerRevolution() {
+ return 4096.0;
+ }
+ static constexpr double kProximalEncoderRatio() { return (15.0 / 95.0); }
+ static constexpr double kMaxProximalEncoderPulsesPerSecond() {
+ return control_loops::superstructure::arm::kArmConstants.free_speed /
+ (2.0 * M_PI) / control_loops::superstructure::arm::kArmConstants.g1 /
+ kProximalEncoderRatio() * kProximalEncoderCountsPerRevolution();
+ }
+ static constexpr double kProximalPotRatio() {
+ return (24.0 / 36.0) * (24.0 / 58.0) * (15.0 / 95.0);
+ }
+
+ static constexpr double kDistalEncoderCountsPerRevolution() { return 4096.0; }
+ static constexpr double kDistalEncoderRatio() { return (15.0 / 95.0); }
+ static constexpr double kMaxDistalEncoderPulsesPerSecond() {
+ return control_loops::superstructure::arm::kArmConstants.free_speed /
+ (2.0 * M_PI) / control_loops::superstructure::arm::kArmConstants.g2 /
+ kDistalEncoderRatio() * kProximalEncoderCountsPerRevolution();
+ }
+ static constexpr double kDistalPotRatio() {
+ return (24.0 / 36.0) * (18.0 / 66.0) * (15.0 / 95.0);
+ }
struct PotConstants {
::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemParams<
@@ -65,6 +89,14 @@
subsystem_params;
double potentiometer_offset;
};
+
+ struct ArmJointConstants {
+ ::frc971::constants::PotAndAbsoluteEncoderZeroingConstants zeroing;
+ double potentiometer_offset;
+ };
+
+ ArmJointConstants arm_proximal;
+ ArmJointConstants arm_distal;
};
// Creates and returns a Values instance for the constants.
diff --git a/y2023/control_loops/python/graph_codegen.py b/y2023/control_loops/python/graph_codegen.py
index 06f036e..667a78d 100644
--- a/y2023/control_loops/python/graph_codegen.py
+++ b/y2023/control_loops/python/graph_codegen.py
@@ -27,11 +27,11 @@
cc_file.append(" %s," % (alpha_unitizer))
if reverse:
cc_file.append(
- " Trajectory(dynamics, Path::Reversed(%s()), 0.005, kArmConstants));"
+ " Trajectory(dynamics, Path::Reversed(%s()), 0.005));"
% (path_function_name(str(name))))
else:
cc_file.append(
- " Trajectory(dynamics, %s(), 0.005, kArmConstants));"
+ " Trajectory(dynamics, %s(), 0.005));"
% (path_function_name(str(name))))
start_index = None
@@ -202,11 +202,11 @@
h_file.append(" double vmax);")
cc_file.append("SearchGraph MakeSearchGraph("
"const frc971::control_loops::arm::Dynamics *dynamics, "
- "::std::vector<TrajectoryAndParams> * /*trajectories*/,")
+ "::std::vector<TrajectoryAndParams> *trajectories,")
cc_file.append(" "
- "const ::Eigen::Matrix<double, 2, 2> & /*alpha_unitizer*/,")
+ "const ::Eigen::Matrix<double, 2, 2> &alpha_unitizer,")
cc_file.append(" "
- "double /*vmax*/) {")
+ "double vmax) {")
cc_file.append(" ::std::vector<SearchGraph::Edge> edges;")
index = 0
diff --git a/y2023/control_loops/python/graph_paths.py b/y2023/control_loops/python/graph_paths.py
index 6d8f489..cfc68ac 100644
--- a/y2023/control_loops/python/graph_paths.py
+++ b/y2023/control_loops/python/graph_paths.py
@@ -17,16 +17,21 @@
circular_index=-1)
cone_perch_pos = to_theta_with_circular_index(1.0, 2.0, circular_index=-1)
-points = []
+# TODO(Max): Add real paths for arm.
+points = [(neutral, "NeutralPos"), (neutral_to_cone_1, "NeutralToConePos1"),
+ (neutral_to_cone_2, "NeutralToConePos2"), (cone_pos, "ConePos"),
+ (neutral_to_cone_perch_pos_1, "NeutralToConePerchPos1"),
+ (neutral_to_cone_perch_pos_2, "NeutralToConePerchPos2"),
+ (cone_perch_pos, "ConePerchPos")]
front_points = []
back_points = []
unnamed_segments = []
-named_segments = []
-
-segments = [
+named_segments = [
ThetaSplineSegment(neutral, neutral_to_cone_1, neutral_to_cone_2, cone_pos,
"NeutralToCone"),
ThetaSplineSegment(neutral, neutral_to_cone_perch_pos_1,
neutral_to_cone_perch_pos_2, cone_perch_pos,
- "NeutralToPerchedCone"),
+ "NeutralToConePerch"),
]
+
+segments = unnamed_segments + named_segments
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_
diff --git a/y2023/control_loops/superstructure/superstructure_position.fbs b/y2023/control_loops/superstructure/superstructure_position.fbs
index dc3679d..1c8c0e9 100644
--- a/y2023/control_loops/superstructure/superstructure_position.fbs
+++ b/y2023/control_loops/superstructure/superstructure_position.fbs
@@ -2,22 +2,26 @@
namespace y2023.control_loops.superstructure;
-table Position {
- // Zero for proximal is facing parallel to base of robot
- // Positive position would be rotated upwards
- proximal:frc971.PotAndAbsolutePosition (id: 0);
+table ArmPosition {
+ // Values of the encoder and potentiometer at the base of the proximal
+ // (connected to drivebase) arm in radians.
+ proximal:frc971.PotAndAbsolutePosition (id: 0);
- // Zero for distal is facing parallel relative to the shoulder
- // Positive position would be rotated upwards
- distal:frc971.PotAndAbsolutePosition (id: 1);
+ // Values of the encoder and potentiometer at the base of the distal
+ // (connected to proximal) arm in radians.
+ distal:frc971.PotAndAbsolutePosition (id: 1);
+}
+
+table Position {
+ arm:ArmPosition (id: 0);
// Zero for roll joint is up vertically
// Positive position would be rotated counterclockwise relative to the robot
- roll_joint:frc971.PotAndAbsolutePosition (id: 2);
+ roll_joint:frc971.PotAndAbsolutePosition (id: 1);
// Zero for wrist is facing staright outward.
// Positive position would be upwards
- wrist:frc971.PotAndAbsolutePosition (id: 3);
+ wrist:frc971.PotAndAbsolutePosition (id: 2);
}
root_type Position;
diff --git a/y2023/control_loops/superstructure/superstructure_status.fbs b/y2023/control_loops/superstructure/superstructure_status.fbs
index ff09317..b2cad0c 100644
--- a/y2023/control_loops/superstructure/superstructure_status.fbs
+++ b/y2023/control_loops/superstructure/superstructure_status.fbs
@@ -3,6 +3,53 @@
namespace y2023.control_loops.superstructure;
+enum ArmState : ubyte {
+ UNINITIALIZED = 0,
+ ZEROING = 1,
+ DISABLED = 2,
+ GOTO_PATH = 3,
+ RUNNING = 4,
+ ESTOP = 5,
+}
+
+table ArmStatus {
+ // State of the estimators.
+ proximal_estimator_state:frc971.PotAndAbsoluteEncoderEstimatorState (id: 0);
+ distal_estimator_state:frc971.PotAndAbsoluteEncoderEstimatorState (id: 1);
+
+ // The node we are currently going to.
+ current_node:uint32 (id: 2);
+ // Distance (in radians) to the end of the path.
+ path_distance_to_go:float (id: 3);
+ // Goal position and velocity (radians)
+ goal_theta0:float (id: 4);
+ goal_theta1:float (id: 5);
+ goal_omega0:float (id: 6);
+ goal_omega1:float (id: 7);
+
+ // Current position and velocity (radians)
+ theta0:float (id: 8);
+ theta1:float (id: 9);
+
+ omega0:float (id: 10);
+ omega1:float (id: 11);
+
+ // Estimated voltage error for the two joints.
+ voltage_error0:float (id: 12);
+ voltage_error1:float (id: 13);
+
+ // True if we are zeroed.
+ zeroed:bool (id: 14);
+
+ // True if the arm is zeroed.
+ estopped:bool (id: 15);
+
+ // The current state machine state.
+ state:ArmState (id: 16);
+
+ // The number of times the LQR solver failed.
+ failed_solutions:uint32 (id: 17);
+}
table Status {
// All subsystems know their location.
@@ -11,13 +58,11 @@
// If true, we have aborted. This is the or of all subsystem estops.
estopped:bool (id: 1);
- proximal:frc971.PotAndAbsoluteEncoderEstimatorState (id: 2);
+ arm:ArmStatus (id: 2);
- distal:frc971.PotAndAbsoluteEncoderEstimatorState (id: 3);
+ roll_joint:frc971.PotAndAbsoluteEncoderEstimatorState (id: 3);
- roll_joint:frc971.PotAndAbsoluteEncoderEstimatorState (id: 4);
-
- wrist:frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus (id: 5);
+ wrist:frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus (id: 4);
}
root_type Status;