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);
}
diff --git a/y2023/joystick_reader.cc b/y2023/joystick_reader.cc
index 3caaa41..adeda2b 100644
--- a/y2023/joystick_reader.cc
+++ b/y2023/joystick_reader.cc
@@ -30,6 +30,7 @@
using frc971::input::driver_station::ControlBit;
using frc971::input::driver_station::JoystickAxis;
using frc971::input::driver_station::POVLocation;
+using y2023::control_loops::superstructure::RollerGoal;
namespace y2023 {
namespace input {
@@ -66,16 +67,15 @@
return;
}
- bool intake = false;
- bool spit = false;
+ RollerGoal roller_goal = RollerGoal::IDLE;
// TODO(milind): add more actions and paths
if (data.IsPressed(kIntake)) {
- intake = true;
- arm_goal_position_ = arm::PickupPosIndex();
+ roller_goal = RollerGoal::INTAKE;
+ arm_goal_position_ = arm::ScorePosIndex();
} else if (data.IsPressed(kSpit)) {
- spit = true;
- arm_goal_position_ = arm::PickupPosIndex();
+ roller_goal = RollerGoal::SPIT;
+ arm_goal_position_ = arm::ScorePosIndex();
} else if (data.IsPressed(kScore)) {
arm_goal_position_ = arm::ScorePosIndex();
}
@@ -86,8 +86,7 @@
superstructure::Goal::Builder superstructure_goal_builder =
builder.MakeBuilder<superstructure::Goal>();
superstructure_goal_builder.add_arm_goal_position(arm_goal_position_);
- superstructure_goal_builder.add_intake(intake);
- superstructure_goal_builder.add_spit(spit);
+ superstructure_goal_builder.add_roller_goal(roller_goal);
if (builder.Send(superstructure_goal_builder.Finish()) !=
aos::RawSender::Error::kOk) {
AOS_LOG(ERROR, "Sending superstructure goal failed.\n");