Add wrist logic and tests in superstructure
Signed-off-by: Maxwell Henderson <mxwhenderson@gmail.com>
Change-Id: Ib71a65aacfc37858e8111a310dd3a97d308fb61c
diff --git a/y2023/constants.cc b/y2023/constants.cc
index e2e8522..0809f56 100644
--- a/y2023/constants.cc
+++ b/y2023/constants.cc
@@ -47,20 +47,22 @@
roll_joint->zeroing.moving_buffer_size = 20;
roll_joint->zeroing.allowable_encoder_error = 0.9;
- wrist->zeroing_voltage = 3.0;
- wrist->operating_voltage = 12.0;
- wrist->zeroing_profile_params = {0.5, 3.0};
- wrist->default_profile_params = {6.0, 30.0};
- wrist->range = Values::kWristRange();
- wrist->make_integral_loop =
+ wrist->subsystem_params.zeroing_voltage = 3.0;
+ wrist->subsystem_params.operating_voltage = 12.0;
+ wrist->subsystem_params.zeroing_profile_params = {0.5, 3.0};
+ wrist->subsystem_params.default_profile_params = {6.0, 30.0};
+ wrist->subsystem_params.range = Values::kWristRange();
+ wrist->subsystem_params.make_integral_loop =
control_loops::superstructure::wrist::MakeIntegralWristLoop;
- wrist->zeroing_constants.average_filter_size = Values::kZeroingSampleSize;
- wrist->zeroing_constants.one_revolution_distance =
+ wrist->subsystem_params.zeroing_constants.average_filter_size =
+ Values::kZeroingSampleSize;
+ wrist->subsystem_params.zeroing_constants.one_revolution_distance =
M_PI * 2.0 * constants::Values::kWristEncoderRatio();
- wrist->zeroing_constants.zeroing_threshold = 0.0005;
- wrist->zeroing_constants.moving_buffer_size = 20;
- wrist->zeroing_constants.allowable_encoder_error = 0.9;
- wrist->zeroing_constants.middle_position = Values::kWristRange().middle();
+ wrist->subsystem_params.zeroing_constants.zeroing_threshold = 0.0005;
+ wrist->subsystem_params.zeroing_constants.moving_buffer_size = 20;
+ wrist->subsystem_params.zeroing_constants.allowable_encoder_error = 0.9;
+ wrist->subsystem_params.zeroing_constants.middle_position =
+ Values::kWristRange().middle();
switch (team) {
// A set of constants for tests.
@@ -74,7 +76,8 @@
roll_joint->zeroing.measured_absolute_position = 0.0;
roll_joint->potentiometer_offset = 0.0;
- wrist->zeroing_constants.measured_absolute_position = 0.0;
+ wrist->subsystem_params.zeroing_constants.measured_absolute_position =
+ 0.0;
break;
@@ -93,7 +96,8 @@
0.866186131631967 - 0.0256788357596952 + 0.18101759154572017 -
0.0208958996127179;
- wrist->zeroing_constants.measured_absolute_position = 0.0;
+ wrist->subsystem_params.zeroing_constants.measured_absolute_position =
+ 0.0;
break;
@@ -107,7 +111,8 @@
roll_joint->zeroing.measured_absolute_position = 0.0;
roll_joint->potentiometer_offset = 0.0;
- wrist->zeroing_constants.measured_absolute_position = 0.0;
+ wrist->subsystem_params.zeroing_constants.measured_absolute_position =
+ 0.0;
break;
@@ -121,7 +126,8 @@
roll_joint->zeroing.measured_absolute_position = 0.0;
roll_joint->potentiometer_offset = 0.0;
- wrist->zeroing_constants.measured_absolute_position = 0.0;
+ wrist->subsystem_params.zeroing_constants.measured_absolute_position =
+ 0.0;
break;
diff --git a/y2023/constants.h b/y2023/constants.h
index 3c9c4be..3f7a164 100644
--- a/y2023/constants.h
+++ b/y2023/constants.h
@@ -146,12 +146,12 @@
// Game object is fed into end effector for at least this time
static constexpr std::chrono::milliseconds kExtraIntakingTime() {
- return std::chrono::seconds(2);
+ return std::chrono::seconds(2);
}
// Game object is spit from end effector for at least this time
static constexpr std::chrono::milliseconds kExtraSpittingTime() {
- return std::chrono::seconds(2);
+ return std::chrono::seconds(2);
}
struct PotConstants {
@@ -168,6 +168,12 @@
double potentiometer_offset;
};
+ struct AbsEncoderConstants {
+ ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemParams<
+ ::frc971::zeroing::AbsoluteEncoderZeroingEstimator>
+ subsystem_params;
+ };
+
struct ArmJointConstants {
::frc971::constants::PotAndAbsoluteEncoderZeroingConstants zeroing;
double potentiometer_offset;
@@ -177,9 +183,7 @@
ArmJointConstants arm_distal;
ArmJointConstants roll_joint;
- ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemParams<
- ::frc971::zeroing::AbsoluteEncoderZeroingEstimator>
- wrist;
+ AbsEncoderConstants wrist;
};
// Creates and returns a Values instance for the constants.
diff --git a/y2023/control_loops/superstructure/superstructure.cc b/y2023/control_loops/superstructure/superstructure.cc
index 94ff7b8..3b4d2b0 100644
--- a/y2023/control_loops/superstructure/superstructure.cc
+++ b/y2023/control_loops/superstructure/superstructure.cc
@@ -30,7 +30,8 @@
joystick_state_fetcher_(
event_loop->MakeFetcher<aos::JoystickState>("/aos")),
arm_(values_),
- end_effector_() {}
+ end_effector_(),
+ wrist_(values->wrist.subsystem_params) {}
void Superstructure::RunIteration(const Goal *unsafe_goal,
const Position *position,
@@ -65,6 +66,12 @@
output != nullptr ? &output_struct.roll_joint_voltage : nullptr,
status->fbb());
+ flatbuffers::Offset<AbsoluteEncoderProfiledJointStatus> wrist_offset =
+ wrist_.Iterate(unsafe_goal != nullptr ? unsafe_goal->wrist() : nullptr,
+ position->wrist(),
+ output != nullptr ? &output_struct.wrist_voltage : nullptr,
+ status->fbb());
+
EndEffectorState end_effector_state = end_effector_.RunIteration(
timestamp,
unsafe_goal != nullptr ? unsafe_goal->roller_goal() : RollerGoal::IDLE,
@@ -79,6 +86,7 @@
status_builder.add_zeroed(true);
status_builder.add_estopped(false);
status_builder.add_arm(arm_status_offset);
+ status_builder.add_wrist(wrist_offset);
status_builder.add_end_effector_state(end_effector_state);
(void)status->Send(status_builder.Finish());
diff --git a/y2023/control_loops/superstructure/superstructure.h b/y2023/control_loops/superstructure/superstructure.h
index d8f3d59..827cc40 100644
--- a/y2023/control_loops/superstructure/superstructure.h
+++ b/y2023/control_loops/superstructure/superstructure.h
@@ -29,12 +29,21 @@
::frc971::zeroing::PotAndAbsoluteEncoderZeroingEstimator,
::frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus>;
+ using AbsoluteEncoderSubsystem =
+ ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystem<
+ ::frc971::zeroing::AbsoluteEncoderZeroingEstimator,
+ ::frc971::control_loops::AbsoluteEncoderProfiledJointStatus>;
+
explicit Superstructure(::aos::EventLoop *event_loop,
std::shared_ptr<const constants::Values> values,
const ::std::string &name = "/superstructure");
double robot_velocity() const;
+ inline const arm::Arm &arm() const { return arm_; }
+ inline const EndEffector &end_effector() const { return end_effector_; }
+ inline const AbsoluteEncoderSubsystem &wrist() const { return wrist_; }
+
protected:
virtual void RunIteration(const Goal *unsafe_goal, const Position *position,
aos::Sender<Output>::Builder *output,
@@ -49,6 +58,7 @@
arm::Arm arm_;
EndEffector end_effector_;
+ AbsoluteEncoderSubsystem wrist_;
aos::Alliance alliance_ = aos::Alliance::kInvalid;
diff --git a/y2023/control_loops/superstructure/superstructure_lib_test.cc b/y2023/control_loops/superstructure/superstructure_lib_test.cc
index f3f0471..a33c364 100644
--- a/y2023/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2023/control_loops/superstructure/superstructure_lib_test.cc
@@ -44,6 +44,11 @@
frc971::control_loops::RelativeEncoderProfiledJointStatus,
RelativeEncoderSubsystem::State, constants::Values::PotConstants>;
+using AbsoluteEncoderSimulator = ::frc971::control_loops::SubsystemSimulator<
+ ::frc971::control_loops::AbsoluteEncoderProfiledJointStatus,
+ Superstructure::AbsoluteEncoderSubsystem::State,
+ constants::Values::AbsEncoderConstants>;
+
class ArmSimulation {
public:
explicit ArmSimulation(
@@ -169,6 +174,14 @@
dt_(dt),
arm_(values->arm_proximal.zeroing, values->arm_distal.zeroing,
values->roll_joint.zeroing, dt_),
+ wrist_(new CappedTestPlant(wrist::MakeWristPlant()),
+ PositionSensorSimulator(
+ values->wrist.subsystem_params.zeroing_constants
+ .one_revolution_distance),
+ values->wrist, constants::Values::kWristRange(),
+ values->wrist.subsystem_params.zeroing_constants
+ .measured_absolute_position,
+ dt_),
superstructure_position_sender_(
event_loop_->MakeSender<Position>("/superstructure")),
superstructure_status_fetcher_(
@@ -189,6 +202,9 @@
superstructure_output_fetcher_->distal_voltage(),
superstructure_output_fetcher_->roll_joint_voltage())
.finished());
+
+ wrist_.Simulate(superstructure_output_fetcher_->wrist_voltage(),
+ superstructure_status_fetcher_->wrist());
}
first_ = false;
SendPositionMessage();
@@ -200,7 +216,8 @@
arm_.InitializePosition(position);
}
- const ArmSimulation &arm() const { return arm_; }
+ ArmSimulation *arm() { return &arm_; }
+ AbsoluteEncoderSimulator *wrist() { return &wrist_; }
// Sends a queue message with the position of the superstructure.
void SendPositionMessage() {
@@ -210,8 +227,17 @@
flatbuffers::Offset<ArmPosition> arm_offset =
arm_.GetSensorValues(builder.fbb());
+ frc971::AbsolutePosition::Builder wrist_builder =
+ builder.MakeBuilder<frc971::AbsolutePosition>();
+ flatbuffers::Offset<frc971::AbsolutePosition> wrist_offset =
+ wrist_.encoder()->GetSensorValues(&wrist_builder);
+
Position::Builder position_builder = builder.MakeBuilder<Position>();
position_builder.add_arm(arm_offset);
+ position_builder.add_wrist(wrist_offset);
+ // TODO(Max): Add tests for end effector beam breaks
+ position_builder.add_end_effector_cone_beam_break(false);
+ position_builder.add_end_effector_cube_beam_break(false);
CHECK_EQ(builder.Send(position_builder.Finish()),
aos::RawSender::Error::kOk);
}
@@ -222,6 +248,7 @@
::aos::PhasedLoopHandler *phased_loop_handle_ = nullptr;
ArmSimulation arm_;
+ AbsoluteEncoderSimulator wrist_;
::aos::Sender<Position> superstructure_position_sender_;
::aos::Fetcher<Status> superstructure_status_fetcher_;
@@ -309,18 +336,23 @@
superstructure_status_fetcher_->arm()->omega2(), kEpsOmega);
// Check that our simulator matches the status
- EXPECT_NEAR(superstructure_plant_.arm().proximal_position(),
+ EXPECT_NEAR(superstructure_plant_.arm()->proximal_position(),
superstructure_status_fetcher_->arm()->theta0(), kEpsTheta);
- EXPECT_NEAR(superstructure_plant_.arm().distal_position(),
+ EXPECT_NEAR(superstructure_plant_.arm()->distal_position(),
superstructure_status_fetcher_->arm()->theta1(), kEpsTheta);
- EXPECT_NEAR(superstructure_plant_.arm().roll_joint_position(),
+ EXPECT_NEAR(superstructure_plant_.arm()->roll_joint_position(),
superstructure_status_fetcher_->arm()->theta2(), kEpsTheta);
- EXPECT_NEAR(superstructure_plant_.arm().proximal_velocity(),
+ EXPECT_NEAR(superstructure_plant_.arm()->proximal_velocity(),
superstructure_status_fetcher_->arm()->omega0(), kEpsOmega);
- EXPECT_NEAR(superstructure_plant_.arm().distal_velocity(),
+ EXPECT_NEAR(superstructure_plant_.arm()->distal_velocity(),
superstructure_status_fetcher_->arm()->omega1(), kEpsOmega);
- EXPECT_NEAR(superstructure_plant_.arm().roll_joint_velocity(),
+ EXPECT_NEAR(superstructure_plant_.arm()->roll_joint_velocity(),
superstructure_status_fetcher_->arm()->omega2(), kEpsOmega);
+
+ if (superstructure_goal_fetcher_->has_wrist()) {
+ EXPECT_NEAR(superstructure_goal_fetcher_->wrist()->unsafe_goal(),
+ superstructure_status_fetcher_->wrist()->position(), 0.001);
+ }
}
void CheckIfZeroed() {
@@ -398,8 +430,17 @@
{
auto builder = superstructure_goal_sender_.MakeBuilder();
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ wrist_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kWristRange().middle());
+
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_arm_goal_position(arm::NeutralPosIndex());
+ goal_builder.add_trajectory_override(false);
+ goal_builder.add_wrist(wrist_offset);
+ goal_builder.add_roller_goal(RollerGoal::IDLE);
+
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
RunFor(chrono::seconds(10));
@@ -415,9 +456,17 @@
WaitUntilZeroed();
{
auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ wrist_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kWristRange().upper);
+
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_arm_goal_position(arm::NeutralPosIndex());
+ goal_builder.add_trajectory_override(false);
+ goal_builder.add_wrist(wrist_offset);
+ goal_builder.add_roller_goal(RollerGoal::IDLE);
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
@@ -439,8 +488,14 @@
{
auto builder = superstructure_goal_sender_.MakeBuilder();
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ wrist_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kWristRange().upper);
+
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_wrist(wrist_offset);
+
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
RunFor(chrono::seconds(20));
@@ -451,11 +506,21 @@
{
auto builder = superstructure_goal_sender_.MakeBuilder();
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ wrist_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kWristRange().lower,
+ CreateProfileParameters(*builder.fbb(), 20.0, 0.1));
+
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_wrist(wrist_offset);
+
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
+ superstructure_plant_.wrist()->set_peak_velocity(23.0);
+ superstructure_plant_.wrist()->set_peak_acceleration(0.2);
+
// TODO(Milo): Make this a sane time
RunFor(chrono::seconds(20));
VerifyNearGoal();
@@ -466,6 +531,10 @@
SetEnabled(true);
WaitUntilZeroed();
RunFor(chrono::seconds(2));
+
+ EXPECT_EQ(ArmState::RUNNING, superstructure_.arm().state());
+ EXPECT_EQ(Superstructure::AbsoluteEncoderSubsystem::State::RUNNING,
+ superstructure_.wrist().state());
}
// Tests that running disabled works