Combine intake and spit in superstructure goal
With how the superstructure goal worked before there could be a goal to
both intake and spit. So I moved them into an enum with an IDLE state
if its not intaking or spitting.
Signed-off-by: Maxwell Henderson <mxwhenderson@gmail.com>
Change-Id: I51d893a9f62a8a0b9621f89ca01da91f4b9491d1
diff --git a/y2023/control_loops/superstructure/arm/arm.cc b/y2023/control_loops/superstructure/arm/arm.cc
index 5417dc8..07d54ec 100644
--- a/y2023/control_loops/superstructure/arm/arm.cc
+++ b/y2023/control_loops/superstructure/arm/arm.cc
@@ -55,8 +55,8 @@
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,
- double *roll_joint_output, bool /*intake*/, bool /*spit*/,
- flatbuffers::FlatBufferBuilder *fbb) {
+ double *roll_joint_output, flatbuffers::FlatBufferBuilder *fbb) {
+ ::Eigen::Matrix<double, 2, 1> Y;
const bool outputs_disabled =
((proximal_output == nullptr) || (distal_output == nullptr) ||
(roll_joint_output == nullptr));
diff --git a/y2023/control_loops/superstructure/arm/arm.h b/y2023/control_loops/superstructure/arm/arm.h
index 9cda7fc..6e4cc29 100644
--- a/y2023/control_loops/superstructure/arm/arm.h
+++ b/y2023/control_loops/superstructure/arm/arm.h
@@ -47,8 +47,7 @@
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,
- double *roll_joint_output, bool /*intake*/, bool /*spit*/,
- flatbuffers::FlatBufferBuilder *fbb);
+ double *roll_joint_output, flatbuffers::FlatBufferBuilder *fbb);
void Reset();
diff --git a/y2023/control_loops/superstructure/end_effector.cc b/y2023/control_loops/superstructure/end_effector.cc
index 3cf19c9..55a592f 100644
--- a/y2023/control_loops/superstructure/end_effector.cc
+++ b/y2023/control_loops/superstructure/end_effector.cc
@@ -14,7 +14,7 @@
: state_(EndEffectorState::IDLE), beambreak_(false) {}
EndEffectorState EndEffector::RunIteration(
- const ::aos::monotonic_clock::time_point timestamp, bool intake, bool spit,
+ const ::aos::monotonic_clock::time_point timestamp, RollerGoal roller_goal,
bool cone_beambreak, bool cube_beambreak, double *roller_voltage) {
bool beambreak = cone_beambreak || cube_beambreak;
@@ -23,7 +23,7 @@
switch (state_) {
case EndEffectorState::IDLE:
// If idle and intake requested, intake
- if (intake) {
+ if (roller_goal == RollerGoal::INTAKE) {
state_ = EndEffectorState::INTAKING;
timer_ = timestamp;
}
@@ -50,7 +50,7 @@
timer_ = timestamp;
}
// If loaded and spit requested, spit
- else if (spit) {
+ else if (roller_goal == RollerGoal::SPIT) {
state_ = EndEffectorState::SPITTING;
}
break;
diff --git a/y2023/control_loops/superstructure/end_effector.h b/y2023/control_loops/superstructure/end_effector.h
index 7d5d603..d03582c 100644
--- a/y2023/control_loops/superstructure/end_effector.h
+++ b/y2023/control_loops/superstructure/end_effector.h
@@ -17,8 +17,8 @@
public:
EndEffector();
EndEffectorState RunIteration(
- const ::aos::monotonic_clock::time_point timestamp, bool intake,
- bool spit, bool cone_beambreak, bool cube_beambreak,
+ const ::aos::monotonic_clock::time_point timestamp,
+ RollerGoal roller_goal, bool cone_beambreak, bool cube_beambreak,
double *intake_roller_voltage);
EndEffectorState state();
void Reset();
diff --git a/y2023/control_loops/superstructure/superstructure.cc b/y2023/control_loops/superstructure/superstructure.cc
index c59804c..94ff7b8 100644
--- a/y2023/control_loops/superstructure/superstructure.cc
+++ b/y2023/control_loops/superstructure/superstructure.cc
@@ -63,14 +63,11 @@
output != nullptr ? &output_struct.proximal_voltage : nullptr,
output != nullptr ? &output_struct.distal_voltage : nullptr,
output != nullptr ? &output_struct.roll_joint_voltage : nullptr,
- unsafe_goal != nullptr ? unsafe_goal->intake() : false,
- unsafe_goal != nullptr ? unsafe_goal->spit() : false,
-
status->fbb());
EndEffectorState end_effector_state = end_effector_.RunIteration(
- timestamp, unsafe_goal != nullptr ? unsafe_goal->intake() : false,
- unsafe_goal != nullptr ? unsafe_goal->spit() : false,
+ timestamp,
+ unsafe_goal != nullptr ? unsafe_goal->roller_goal() : RollerGoal::IDLE,
position->end_effector_cone_beam_break(),
position->end_effector_cube_beam_break(), &output_struct.roller_voltage);
diff --git a/y2023/control_loops/superstructure/superstructure_goal.fbs b/y2023/control_loops/superstructure/superstructure_goal.fbs
index 936cbee..8f2d5ab 100644
--- a/y2023/control_loops/superstructure/superstructure_goal.fbs
+++ b/y2023/control_loops/superstructure/superstructure_goal.fbs
@@ -2,6 +2,11 @@
namespace y2023.control_loops.superstructure;
+enum RollerGoal: ubyte {
+ IDLE = 0,
+ INTAKE = 1,
+ SPIT = 2,
+}
table Goal {
// Used to identify a position in the planned set of positions on the arm.
@@ -13,11 +18,7 @@
wrist:frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal (id: 2);
- // If this is true, the rollers should intake.
- intake:bool (id: 3);
-
- // If this is true, the rollers should spit.
- spit:bool (id: 4);
+ roller_goal:RollerGoal (id: 3);
}