Add end effector to superstructure
Add superstructure state machine for the end
effector, update flatbuffers to match, and send to
status.
Signed-off-by: Nathan Leong <nathanrleong@gmail.com>
Change-Id: I7f0c60f05a147ba6b3aec1e3488855c8e674c670
diff --git a/y2023/control_loops/superstructure/BUILD b/y2023/control_loops/superstructure/BUILD
index 52254e4..d00489a 100644
--- a/y2023/control_loops/superstructure/BUILD
+++ b/y2023/control_loops/superstructure/BUILD
@@ -60,6 +60,25 @@
)
cc_library(
+ name = "end_effector",
+ srcs = [
+ "end_effector.cc",
+ ],
+ hdrs = [
+ "end_effector.h",
+ ],
+ deps = [
+ ":superstructure_goal_fbs",
+ ":superstructure_position_fbs",
+ ":superstructure_status_fbs",
+ "//aos/events:event_loop",
+ "//aos/time",
+ "//frc971/control_loops:control_loop",
+ "//y2023:constants",
+ ],
+)
+
+cc_library(
name = "superstructure_lib",
srcs = [
"superstructure.cc",
@@ -68,6 +87,7 @@
"superstructure.h",
],
deps = [
+ ":end_effector",
":superstructure_goal_fbs",
":superstructure_output_fbs",
":superstructure_position_fbs",
diff --git a/y2023/control_loops/superstructure/end_effector.cc b/y2023/control_loops/superstructure/end_effector.cc
new file mode 100644
index 0000000..3cf19c9
--- /dev/null
+++ b/y2023/control_loops/superstructure/end_effector.cc
@@ -0,0 +1,84 @@
+#include "y2023/control_loops/superstructure/end_effector.h"
+
+#include "aos/events/event_loop.h"
+#include "aos/time/time.h"
+#include "frc971/control_loops/control_loop.h"
+
+namespace y2023 {
+namespace control_loops {
+namespace superstructure {
+
+using ::aos::monotonic_clock;
+
+EndEffector::EndEffector()
+ : state_(EndEffectorState::IDLE), beambreak_(false) {}
+
+EndEffectorState EndEffector::RunIteration(
+ const ::aos::monotonic_clock::time_point timestamp, bool intake, bool spit,
+ bool cone_beambreak, bool cube_beambreak, double *roller_voltage) {
+ bool beambreak = cone_beambreak || cube_beambreak;
+
+ *roller_voltage = 0.0;
+
+ switch (state_) {
+ case EndEffectorState::IDLE:
+ // If idle and intake requested, intake
+ if (intake) {
+ state_ = EndEffectorState::INTAKING;
+ timer_ = timestamp;
+ }
+ break;
+ case EndEffectorState::INTAKING:
+ // If intaking and beam break is not triggered, keep intaking
+ if (beambreak) {
+ // Beam has been broken, switch to loaded.
+ state_ = EndEffectorState::LOADED;
+ break;
+ } else if (timestamp > timer_ + constants::Values::kExtraIntakingTime()) {
+ // Intaking failed, waited 2 seconds with no beambreak
+ state_ = EndEffectorState::IDLE;
+ break;
+ }
+
+ *roller_voltage = constants::Values::kRollerVoltage();
+
+ break;
+ case EndEffectorState::LOADED:
+ // If loaded and beam break not triggered, intake
+ if (!beambreak) {
+ state_ = EndEffectorState::INTAKING;
+ timer_ = timestamp;
+ }
+ // If loaded and spit requested, spit
+ else if (spit) {
+ state_ = EndEffectorState::SPITTING;
+ }
+ break;
+
+ case EndEffectorState::SPITTING:
+ // If spit requested, spit
+ *roller_voltage = -constants::Values::kRollerVoltage();
+ if (beambreak_) {
+ if (!beambreak) {
+ timer_ = timestamp;
+ }
+ } else if (timestamp > timer_ + constants::Values::kExtraSpittingTime()) {
+ // Finished spitting
+ state_ = EndEffectorState::IDLE;
+ }
+
+ break;
+ }
+
+ beambreak_ = beambreak;
+
+ return state_;
+}
+
+EndEffectorState EndEffector::state() { return state_; }
+
+void EndEffector::Reset() { state_ = EndEffectorState::IDLE; }
+
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2023
diff --git a/y2023/control_loops/superstructure/end_effector.h b/y2023/control_loops/superstructure/end_effector.h
new file mode 100644
index 0000000..7d5d603
--- /dev/null
+++ b/y2023/control_loops/superstructure/end_effector.h
@@ -0,0 +1,37 @@
+#ifndef Y2023_CONTROL_LOOPS_SUPERSTRUCTURE_END_EFFECTOR_H_
+#define Y2023_CONTROL_LOOPS_SUPERSTRUCTURE_END_EFFECTOR_H_
+
+#include "aos/events/event_loop.h"
+#include "aos/time/time.h"
+#include "frc971/control_loops/control_loop.h"
+#include "y2023/constants.h"
+#include "y2023/control_loops/superstructure/superstructure_goal_generated.h"
+#include "y2023/control_loops/superstructure/superstructure_position_generated.h"
+#include "y2023/control_loops/superstructure/superstructure_status_generated.h"
+
+namespace y2023 {
+namespace control_loops {
+namespace superstructure {
+
+class EndEffector {
+ public:
+ EndEffector();
+ EndEffectorState RunIteration(
+ const ::aos::monotonic_clock::time_point timestamp, bool intake,
+ bool spit, bool cone_beambreak, bool cube_beambreak,
+ double *intake_roller_voltage);
+ EndEffectorState state();
+ void Reset();
+
+ private:
+ EndEffectorState state_ = EndEffectorState::IDLE;
+ aos::monotonic_clock::time_point timer_ = aos::monotonic_clock::min_time;
+
+ bool beambreak_;
+};
+
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2023
+
+#endif // Y2023_CONTROL_LOOPS_SUPERSTRUCTURE_END_EFFECTOR_H_
diff --git a/y2023/control_loops/superstructure/superstructure.cc b/y2023/control_loops/superstructure/superstructure.cc
index 1ad2625..c59804c 100644
--- a/y2023/control_loops/superstructure/superstructure.cc
+++ b/y2023/control_loops/superstructure/superstructure.cc
@@ -29,7 +29,8 @@
"/drivetrain")),
joystick_state_fetcher_(
event_loop->MakeFetcher<aos::JoystickState>("/aos")),
- arm_(values_) {}
+ arm_(values_),
+ end_effector_() {}
void Superstructure::RunIteration(const Goal *unsafe_goal,
const Position *position,
@@ -41,6 +42,7 @@
if (WasReset()) {
AOS_LOG(ERROR, "WPILib reset, restarting\n");
arm_.Reset();
+ end_effector_.Reset();
}
OutputT output_struct;
@@ -66,6 +68,12 @@
status->fbb());
+ EndEffectorState end_effector_state = end_effector_.RunIteration(
+ timestamp, unsafe_goal != nullptr ? unsafe_goal->intake() : false,
+ unsafe_goal != nullptr ? unsafe_goal->spit() : false,
+ position->end_effector_cone_beam_break(),
+ position->end_effector_cube_beam_break(), &output_struct.roller_voltage);
+
if (output) {
output->CheckOk(output->Send(Output::Pack(*output->fbb(), &output_struct)));
}
@@ -74,6 +82,7 @@
status_builder.add_zeroed(true);
status_builder.add_estopped(false);
status_builder.add_arm(arm_status_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 2d0fc43..d8f3d59 100644
--- a/y2023/control_loops/superstructure/superstructure.h
+++ b/y2023/control_loops/superstructure/superstructure.h
@@ -6,6 +6,7 @@
#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
#include "y2023/constants.h"
#include "y2023/control_loops/superstructure/arm/arm.h"
+#include "y2023/control_loops/superstructure/end_effector.h"
#include "y2023/control_loops/superstructure/superstructure_goal_generated.h"
#include "y2023/control_loops/superstructure/superstructure_output_generated.h"
#include "y2023/control_loops/superstructure/superstructure_position_generated.h"
@@ -47,6 +48,7 @@
aos::Fetcher<aos::JoystickState> joystick_state_fetcher_;
arm::Arm arm_;
+ EndEffector end_effector_;
aos::Alliance alliance_ = aos::Alliance::kInvalid;
diff --git a/y2023/control_loops/superstructure/superstructure_position.fbs b/y2023/control_loops/superstructure/superstructure_position.fbs
index bd3d607..927fe10 100644
--- a/y2023/control_loops/superstructure/superstructure_position.fbs
+++ b/y2023/control_loops/superstructure/superstructure_position.fbs
@@ -24,6 +24,12 @@
// Zero for wrist is facing staright outward.
// Positive position would be upwards
wrist:frc971.AbsolutePosition (id: 1);
+
+ // If this is true, the cone beam break is triggered.
+ end_effector_cone_beam_break:bool (id: 2);
+
+ // If this is true, the cube beam break is triggered.
+ end_effector_cube_beam_break:bool (id: 3);
}
root_type Position;
diff --git a/y2023/control_loops/superstructure/superstructure_status.fbs b/y2023/control_loops/superstructure/superstructure_status.fbs
index db86687..2555134 100644
--- a/y2023/control_loops/superstructure/superstructure_status.fbs
+++ b/y2023/control_loops/superstructure/superstructure_status.fbs
@@ -57,6 +57,19 @@
failed_solutions:uint32 (id: 17);
}
+// State of the superstructure state machine
+enum EndEffectorState : ubyte {
+ // Not doing anything
+ IDLE = 0,
+ // Intaking the game object into the end effector
+ INTAKING = 1,
+ // The game object is loaded into the end effector
+ LOADED = 2,
+ // Waiting for the arm to be at shooting goal and then telling the
+ // end effector to spit
+ SPITTING = 3,
+}
+
table Status {
// All subsystems know their location.
zeroed:bool (id: 0);
@@ -67,6 +80,8 @@
arm:ArmStatus (id: 2);
wrist:frc971.control_loops.AbsoluteEncoderProfiledJointStatus (id: 3);
+
+ end_effector_state:EndEffectorState (id: 4);
}
root_type Status;