Add pivot joint functionality
Signed-off-by: Charlie Huang <charliehuang09@gmail.com>
Change-Id: Ib40d4ce1a4c91f1b0e75b3548fe43c9218433634
diff --git a/y2023_bot3/control_loops/superstructure/BUILD b/y2023_bot3/control_loops/superstructure/BUILD
index 0330182..8630ef9 100644
--- a/y2023_bot3/control_loops/superstructure/BUILD
+++ b/y2023_bot3/control_loops/superstructure/BUILD
@@ -90,6 +90,7 @@
],
deps = [
":end_effector",
+ ":pivot_joint",
":superstructure_goal_fbs",
":superstructure_output_fbs",
":superstructure_position_fbs",
@@ -188,3 +189,21 @@
"//aos/network:team_number",
],
)
+
+cc_library(
+ name = "pivot_joint",
+ srcs = [
+ "pivot_joint.cc",
+ ],
+ hdrs = [
+ "pivot_joint.h",
+ ],
+ deps = [
+ ":superstructure_goal_fbs",
+ ":superstructure_status_fbs",
+ "//aos/events:event_loop",
+ "//aos/time",
+ "//frc971/control_loops:control_loop",
+ "//y2023_bot3:constants",
+ ],
+)
diff --git a/y2023_bot3/control_loops/superstructure/pivot_joint.cc b/y2023_bot3/control_loops/superstructure/pivot_joint.cc
new file mode 100644
index 0000000..7669396
--- /dev/null
+++ b/y2023_bot3/control_loops/superstructure/pivot_joint.cc
@@ -0,0 +1,75 @@
+#include "pivot_joint.h"
+
+#include "aos/events/event_loop.h"
+#include "aos/time/time.h"
+#include "frc971/control_loops/control_loop.h"
+#include "y2023_bot3/constants.h"
+#include "y2023_bot3/control_loops/superstructure/superstructure_goal_generated.h"
+#include "y2023_bot3/control_loops/superstructure/superstructure_status_generated.h"
+
+namespace y2023_bot3 {
+namespace control_loops {
+namespace superstructure {
+
+PivotJoint::PivotJoint(std::shared_ptr<const constants::Values> values)
+ : pivot_joint_(values->pivot_joint.subsystem_params) {}
+
+flatbuffers::Offset<
+ frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus>
+PivotJoint::RunIteration(PivotGoal goal, double *output,
+ const frc971::PotAndAbsolutePosition *position,
+ flatbuffers::FlatBufferBuilder *status_fbb) {
+ double pivot_goal = 0;
+ switch (goal) {
+ case PivotGoal::NEUTRAL:
+ pivot_goal = 0;
+ break;
+
+ case PivotGoal::PICKUP_FRONT:
+ pivot_goal = 0.25;
+ break;
+
+ case PivotGoal::PICKUP_BACK:
+ pivot_goal = 0.30;
+ break;
+
+ case PivotGoal::SCORE_LOW_FRONT:
+ pivot_goal = 0.35;
+ break;
+
+ case PivotGoal::SCORE_LOW_BACK:
+ pivot_goal = 0.40;
+ break;
+
+ case PivotGoal::SCORE_MID_FRONT:
+ pivot_goal = 0.45;
+ break;
+
+ case PivotGoal::SCORE_MID_BACK:
+ pivot_goal = 0.5;
+ break;
+ }
+
+ flatbuffers::Offset<
+ frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal>
+ pivot_joint_offset = frc971::control_loops::
+ CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *status_fbb, pivot_goal,
+ frc971::CreateProfileParameters(*status_fbb, 12.0, 90.0));
+
+ status_fbb->Finish(pivot_joint_offset);
+
+ flatbuffers::Offset<
+ frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus>
+ pivot_joint_status_offset = pivot_joint_.Iterate(
+ flatbuffers::GetRoot<frc971::control_loops::
+ StaticZeroingSingleDOFProfiledSubsystemGoal>(
+ status_fbb->GetBufferPointer()),
+ position, output, status_fbb);
+
+ return pivot_joint_status_offset;
+}
+
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2023_bot3
diff --git a/y2023_bot3/control_loops/superstructure/pivot_joint.h b/y2023_bot3/control_loops/superstructure/pivot_joint.h
new file mode 100644
index 0000000..1eff122
--- /dev/null
+++ b/y2023_bot3/control_loops/superstructure/pivot_joint.h
@@ -0,0 +1,45 @@
+#ifndef Y2023_BOT3_CONTROL_LOOPS_SUPERSTRUCTURE_PIVOT_JOINT_PIVOT_JOINT_H_
+#define Y2023_BOT3_CONTROL_LOOPS_SUPERSTRUCTURE_PIVOT_JOINT_PIVOT_JOINT_H_
+
+#include "aos/events/event_loop.h"
+#include "aos/time/time.h"
+#include "frc971/control_loops/control_loop.h"
+#include "y2023_bot3/constants.h"
+#include "y2023_bot3/control_loops/superstructure/superstructure_goal_generated.h"
+#include "y2023_bot3/control_loops/superstructure/superstructure_status_generated.h"
+
+namespace y2023_bot3 {
+namespace control_loops {
+namespace superstructure {
+
+class PivotJoint {
+ using PotAndAbsoluteEncoderSubsystem =
+ ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystem<
+ ::frc971::zeroing::PotAndAbsoluteEncoderZeroingEstimator,
+ ::frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus>;
+
+ public:
+ PivotJoint(std::shared_ptr<const constants::Values> values);
+
+ flatbuffers::Offset<
+ frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus>
+ RunIteration(PivotGoal goal, double *output,
+ const frc971::PotAndAbsolutePosition *position,
+ flatbuffers::FlatBufferBuilder *status_fbb);
+
+ bool zeroed() const { return pivot_joint_.zeroed(); }
+
+ bool estopped() const { return pivot_joint_.estopped(); }
+
+ // variable which records the last time at which "intake" button was pressed
+ aos::monotonic_clock::time_point timer_;
+
+ private:
+ PotAndAbsoluteEncoderSubsystem pivot_joint_;
+};
+
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2023_bot3
+
+#endif // Y2023_BOT3_CONTROL_LOOPS_SUPERSTRUCTURE_PIVOT_JOINT_PIVOT_JOINT_H_
diff --git a/y2023_bot3/control_loops/superstructure/superstructure.cc b/y2023_bot3/control_loops/superstructure/superstructure.cc
index 3e88a6c..3b4835c 100644
--- a/y2023_bot3/control_loops/superstructure/superstructure.cc
+++ b/y2023_bot3/control_loops/superstructure/superstructure.cc
@@ -23,7 +23,9 @@
const ::std::string &name)
: frc971::controls::ControlLoop<Goal, Position, Status, Output>(event_loop,
name),
- values_(values) {
+ values_(values),
+ end_effector_(),
+ pivot_joint_(values) {
event_loop->SetRuntimeRealtimePriority(30);
}
@@ -47,15 +49,25 @@
position->end_effector_cube_beam_break(), &output_struct.roller_voltage,
unsafe_goal != nullptr ? unsafe_goal->preloaded_with_cube() : false);
- if (output) {
- output->CheckOk(output->Send(Output::Pack(*output->fbb(), &output_struct)));
- }
+ flatbuffers::Offset<
+ frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus>
+ pivot_joint_offset = pivot_joint_.RunIteration(
+ unsafe_goal != nullptr ? unsafe_goal->pivot_goal()
+ : PivotGoal::NEUTRAL,
+ &(output_struct.pivot_joint_voltage),
+ position->pivot_joint_position(), status->fbb());
Status::Builder status_builder = status->MakeBuilder<Status>();
- status_builder.add_zeroed(true);
+ status_builder.add_zeroed(pivot_joint_.zeroed());
+ status_builder.add_estopped(pivot_joint_.estopped());
+ status_builder.add_pivot_joint(pivot_joint_offset);
status_builder.add_end_effector_state(end_effector_.state());
+ if (output) {
+ output->CheckOk(output->Send(Output::Pack(*output->fbb(), &output_struct)));
+ }
+
(void)status->Send(status_builder.Finish());
}
diff --git a/y2023_bot3/control_loops/superstructure/superstructure.h b/y2023_bot3/control_loops/superstructure/superstructure.h
index 212ced8..efc95a8 100644
--- a/y2023_bot3/control_loops/superstructure/superstructure.h
+++ b/y2023_bot3/control_loops/superstructure/superstructure.h
@@ -10,6 +10,7 @@
#include "y2023_bot3/constants.h"
#include "y2023_bot3/constants/constants_generated.h"
#include "y2023_bot3/control_loops/superstructure/end_effector.h"
+#include "y2023_bot3/control_loops/superstructure/pivot_joint.h"
#include "y2023_bot3/control_loops/superstructure/superstructure_goal_generated.h"
#include "y2023_bot3/control_loops/superstructure/superstructure_output_generated.h"
#include "y2023_bot3/control_loops/superstructure/superstructure_position_generated.h"
@@ -49,6 +50,8 @@
aos::Alliance alliance_ = aos::Alliance::kInvalid;
+ PivotJoint pivot_joint_;
+
DISALLOW_COPY_AND_ASSIGN(Superstructure);
};
diff --git a/y2023_bot3/control_loops/superstructure/superstructure_lib_test.cc b/y2023_bot3/control_loops/superstructure/superstructure_lib_test.cc
index 7387ca7..1150e06 100644
--- a/y2023_bot3/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2023_bot3/control_loops/superstructure/superstructure_lib_test.cc
@@ -31,6 +31,11 @@
using ::frc971::control_loops::PositionSensorSimulator;
using ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal;
using DrivetrainStatus = ::frc971::control_loops::drivetrain::Status;
+using PotAndAbsoluteEncoderSimulator =
+ frc971::control_loops::SubsystemSimulator<
+ frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus,
+ Superstructure::PotAndAbsoluteEncoderSubsystem::State,
+ constants::Values::PotAndAbsEncoderConstants>;
// Class which simulates the superstructure and sends out queue messages with
// the position.
@@ -45,7 +50,15 @@
superstructure_status_fetcher_(
event_loop_->MakeFetcher<Status>("/superstructure")),
superstructure_output_fetcher_(
- event_loop_->MakeFetcher<Output>("/superstructure")) {
+ event_loop_->MakeFetcher<Output>("/superstructure")),
+ pivot_joint_(new CappedTestPlant(pivot_joint::MakePivotJointPlant()),
+ PositionSensorSimulator(
+ values->pivot_joint.subsystem_params.zeroing_constants
+ .one_revolution_distance),
+ values->pivot_joint, constants::Values::kPivotJointRange(),
+ values->pivot_joint.subsystem_params.zeroing_constants
+ .measured_absolute_position,
+ dt) {
(void)values;
phased_loop_handle_ = event_loop_->AddPhasedLoop(
[this](int) {
@@ -53,6 +66,10 @@
if (!first_) {
EXPECT_TRUE(superstructure_output_fetcher_.Fetch());
EXPECT_TRUE(superstructure_status_fetcher_.Fetch());
+
+ pivot_joint_.Simulate(
+ superstructure_output_fetcher_->pivot_joint_voltage(),
+ superstructure_status_fetcher_->pivot_joint());
}
first_ = false;
SendPositionMessage();
@@ -65,9 +82,16 @@
::aos::Sender<Position>::Builder builder =
superstructure_position_sender_.MakeBuilder();
+ frc971::PotAndAbsolutePosition::Builder pivot_joint_builder =
+ builder.MakeBuilder<frc971::PotAndAbsolutePosition>();
+ flatbuffers::Offset<frc971::PotAndAbsolutePosition> pivot_joint_offset =
+ pivot_joint_.encoder()->GetSensorValues(&pivot_joint_builder);
+
Position::Builder position_builder = builder.MakeBuilder<Position>();
position_builder.add_end_effector_cube_beam_break(
end_effector_cube_beam_break_);
+ position_builder.add_pivot_joint_position(pivot_joint_offset);
+
CHECK_EQ(builder.Send(position_builder.Finish()),
aos::RawSender::Error::kOk);
}
@@ -76,6 +100,8 @@
end_effector_cube_beam_break_ = triggered;
}
+ PotAndAbsoluteEncoderSimulator *pivot_joint() { return &pivot_joint_; }
+
private:
::aos::EventLoop *event_loop_;
::aos::PhasedLoopHandler *phased_loop_handle_ = nullptr;
@@ -86,6 +112,8 @@
::aos::Fetcher<Status> superstructure_status_fetcher_;
::aos::Fetcher<Output> superstructure_output_fetcher_;
+ PotAndAbsoluteEncoderSimulator pivot_joint_;
+
bool first_ = true;
};
@@ -139,6 +167,44 @@
ASSERT_TRUE(superstructure_goal_fetcher_.get() != nullptr) << ": No goal";
ASSERT_TRUE(superstructure_status_fetcher_.get() != nullptr)
<< ": No status";
+
+ if (superstructure_goal_fetcher_->has_pivot_goal()) {
+ double pivot_goal = 0.0;
+
+ switch (superstructure_goal_fetcher_->pivot_goal()) {
+ case PivotGoal::NEUTRAL:
+ pivot_goal = 0;
+ break;
+
+ case PivotGoal::PICKUP_FRONT:
+ pivot_goal = 0.25;
+ break;
+
+ case PivotGoal::PICKUP_BACK:
+ pivot_goal = 0.30;
+ break;
+
+ case PivotGoal::SCORE_LOW_FRONT:
+ pivot_goal = 0.35;
+ break;
+
+ case PivotGoal::SCORE_LOW_BACK:
+ pivot_goal = 0.40;
+ break;
+
+ case PivotGoal::SCORE_MID_FRONT:
+ pivot_goal = 0.45;
+ break;
+
+ case PivotGoal::SCORE_MID_BACK:
+ pivot_goal = 0.5;
+ break;
+ }
+
+ EXPECT_NEAR(pivot_goal,
+ superstructure_status_fetcher_->pivot_joint()->position(),
+ 0.001);
+ }
}
void CheckIfZeroed() {
@@ -250,6 +316,137 @@
CheckIfZeroed();
}
+TEST_F(SuperstructureTest, PivotGoal) {
+ SetEnabled(true);
+ WaitUntilZeroed();
+
+ PivotGoal pivot_goal = PivotGoal::PICKUP_FRONT;
+
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_pivot_goal(pivot_goal);
+
+ builder.CheckOk(builder.Send(goal_builder.Finish()));
+ }
+
+ RunFor(dt() * 30);
+
+ ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
+ ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
+
+ VerifyNearGoal();
+
+ pivot_goal = PivotGoal::PICKUP_BACK;
+
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_pivot_goal(pivot_goal);
+
+ builder.CheckOk(builder.Send(goal_builder.Finish()));
+ }
+
+ RunFor(dt() * 30);
+
+ ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
+ ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
+
+ VerifyNearGoal();
+
+ pivot_goal = PivotGoal::SCORE_LOW_FRONT;
+
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_pivot_goal(pivot_goal);
+
+ builder.CheckOk(builder.Send(goal_builder.Finish()));
+ }
+
+ RunFor(dt() * 30);
+
+ ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
+ ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
+
+ VerifyNearGoal();
+
+ pivot_goal = PivotGoal::SCORE_LOW_BACK;
+
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_pivot_goal(pivot_goal);
+
+ builder.CheckOk(builder.Send(goal_builder.Finish()));
+ }
+
+ RunFor(dt() * 30);
+
+ ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
+ ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
+
+ VerifyNearGoal();
+
+ pivot_goal = PivotGoal::SCORE_MID_FRONT;
+
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_pivot_goal(pivot_goal);
+
+ builder.CheckOk(builder.Send(goal_builder.Finish()));
+ }
+
+ RunFor(dt() * 30);
+
+ ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
+ ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
+
+ VerifyNearGoal();
+
+ pivot_goal = PivotGoal::SCORE_MID_BACK;
+
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_pivot_goal(pivot_goal);
+
+ builder.CheckOk(builder.Send(goal_builder.Finish()));
+ }
+
+ RunFor(dt() * 30);
+
+ ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
+ ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
+
+ VerifyNearGoal();
+
+ pivot_goal = PivotGoal::NEUTRAL;
+
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_pivot_goal(pivot_goal);
+
+ builder.CheckOk(builder.Send(goal_builder.Finish()));
+ }
+
+ RunFor(dt() * 30);
+
+ ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
+ ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
+
+ VerifyNearGoal();
+}
+
TEST_F(SuperstructureTest, EndEffectorGoal) {
SetEnabled(true);
WaitUntilZeroed();
diff --git a/y2023_bot3/control_loops/superstructure/superstructure_status.fbs b/y2023_bot3/control_loops/superstructure/superstructure_status.fbs
index f3df83c..3d4947a 100644
--- a/y2023_bot3/control_loops/superstructure/superstructure_status.fbs
+++ b/y2023_bot3/control_loops/superstructure/superstructure_status.fbs
@@ -25,8 +25,8 @@
// If true, we have aborted. This is the or of all subsystem estops.
estopped:bool (id: 1);
- // The current state of the arm.
- pivot_joint_state:frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus (id: 2);
+ // The current state of the pivot.
+ pivot_joint:frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus (id: 2);
end_effector_state:EndEffectorState (id: 3);
}