Third robot end effector
Signed-off-by: Logan Isaacson <100030671@mvla.net>
Change-Id: Iba7b08c0ecd5828542ef769d7a85cd0f3737f5bf
diff --git a/y2023_bot3/control_loops/superstructure/BUILD b/y2023_bot3/control_loops/superstructure/BUILD
index e6a3082..0330182 100644
--- a/y2023_bot3/control_loops/superstructure/BUILD
+++ b/y2023_bot3/control_loops/superstructure/BUILD
@@ -54,12 +54,31 @@
gen_reflections = 1,
deps = [
"//frc971:can_configuration_fbs",
+ "//frc971/control_loops:can_falcon_fbs",
"//frc971/control_loops:control_loops_fbs",
"//frc971/control_loops:profiled_subsystem_fbs",
],
)
cc_library(
+ name = "end_effector",
+ srcs = [
+ "end_effector.cc",
+ ],
+ hdrs = [
+ "end_effector.h",
+ ],
+ deps = [
+ ":superstructure_goal_fbs",
+ ":superstructure_status_fbs",
+ "//aos/events:event_loop",
+ "//aos/time",
+ "//frc971/control_loops:control_loop",
+ "//y2023_bot3:constants",
+ ],
+)
+
+cc_library(
name = "superstructure_lib",
srcs = [
"superstructure.cc",
@@ -70,6 +89,7 @@
data = [
],
deps = [
+ ":end_effector",
":superstructure_goal_fbs",
":superstructure_output_fbs",
":superstructure_position_fbs",
diff --git a/y2023_bot3/control_loops/superstructure/end_effector.cc b/y2023_bot3/control_loops/superstructure/end_effector.cc
new file mode 100644
index 0000000..a98d06f
--- /dev/null
+++ b/y2023_bot3/control_loops/superstructure/end_effector.cc
@@ -0,0 +1,88 @@
+#include "y2023_bot3/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_bot3 {
+namespace control_loops {
+namespace superstructure {
+
+using ::aos::monotonic_clock;
+
+EndEffector::EndEffector()
+ : state_(EndEffectorState::IDLE), timer_(aos::monotonic_clock::min_time) {}
+
+void EndEffector::RunIteration(
+ const ::aos::monotonic_clock::time_point timestamp, RollerGoal roller_goal,
+ bool beambreak_status, double *roller_voltage, bool preloaded_with_cube) {
+ *roller_voltage = 0.0;
+
+ if (roller_goal == RollerGoal::SPIT) {
+ state_ = EndEffectorState::SPITTING;
+ }
+
+ if (roller_goal == RollerGoal::SPIT_MID) {
+ state_ = EndEffectorState::SPITTING_MID;
+ }
+
+ // If we started off preloaded, skip to the loaded state.
+ // Make sure we weren't already there just in case.
+ if (preloaded_with_cube) {
+ switch (state_) {
+ case EndEffectorState::IDLE:
+ case EndEffectorState::INTAKING:
+ state_ = EndEffectorState::LOADED;
+ break;
+ case EndEffectorState::LOADED:
+ break;
+ case EndEffectorState::SPITTING:
+ break;
+ case EndEffectorState::SPITTING_MID:
+ break;
+ }
+ }
+
+ switch (state_) {
+ case EndEffectorState::IDLE:
+ // If idle and intake requested, intake
+ if (roller_goal == RollerGoal::INTAKE_CUBE) {
+ state_ = EndEffectorState::INTAKING;
+ timer_ = timestamp;
+ }
+ break;
+ case EndEffectorState::INTAKING:
+ // If intaking and beam break is not triggered, keep intaking
+ if (roller_goal == RollerGoal::INTAKE_CUBE) {
+ timer_ = timestamp;
+ }
+
+ if (beambreak_status) {
+ // Beam has been broken, switch to loaded.
+ state_ = EndEffectorState::LOADED;
+ break;
+ } else if (timestamp > timer_ + constants::Values::kExtraIntakingTime()) {
+ // Intaking failed, waited 1 second with no beambreak
+ state_ = EndEffectorState::IDLE;
+ break;
+ }
+
+ *roller_voltage = kRollerCubeSuckVoltage();
+
+ break;
+ case EndEffectorState::LOADED:
+ timer_ = timestamp;
+ break;
+ case EndEffectorState::SPITTING:
+ *roller_voltage = kRollerCubeSpitVoltage();
+ break;
+ case EndEffectorState::SPITTING_MID:
+ *roller_voltage = kRollerCubeSpitMidVoltage();
+ }
+}
+
+void EndEffector::Reset() { state_ = EndEffectorState::IDLE; }
+
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2023_bot3
\ No newline at end of file
diff --git a/y2023_bot3/control_loops/superstructure/end_effector.h b/y2023_bot3/control_loops/superstructure/end_effector.h
new file mode 100644
index 0000000..ab4e117
--- /dev/null
+++ b/y2023_bot3/control_loops/superstructure/end_effector.h
@@ -0,0 +1,39 @@
+#ifndef Y2023_BOT3_CONTROL_LOOPS_SUPERSTRUCTURE_END_EFFECTOR_H_
+#define Y2023_BOT3_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_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 EndEffector {
+ public:
+ static constexpr double kRollerCubeSuckVoltage() { return -7.0; }
+ static constexpr double kRollerCubeSpitVoltage() { return 3.0; }
+ static constexpr double kRollerCubeSpitMidVoltage() { return 5.0; }
+
+ EndEffector();
+ void RunIteration(const ::aos::monotonic_clock::time_point timestamp,
+ RollerGoal roller_goal, bool beambreak_status,
+ double *intake_roller_voltage, bool preloaded_with_cube);
+ EndEffectorState state() const { return state_; }
+ void Reset();
+
+ private:
+ EndEffectorState state_;
+
+ // variable which records the last time at which "intake" button was pressed
+ aos::monotonic_clock::time_point timer_;
+};
+
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2023_bot3
+
+#endif // Y2023_CONTROL_LOOPS_SUPERSTRUCTURE_END_EFFECTOR_H_
diff --git a/y2023_bot3/control_loops/superstructure/superstructure.h b/y2023_bot3/control_loops/superstructure/superstructure.h
index 204bb2c..4b3d488 100644
--- a/y2023_bot3/control_loops/superstructure/superstructure.h
+++ b/y2023_bot3/control_loops/superstructure/superstructure.h
@@ -9,6 +9,7 @@
#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
#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/superstructure_goal_generated.h"
#include "y2023_bot3/control_loops/superstructure/superstructure_output_generated.h"
#include "y2023_bot3/control_loops/superstructure/superstructure_position_generated.h"
@@ -27,6 +28,8 @@
double robot_velocity() const;
+ inline const EndEffector &end_effector() const { return end_effector_; }
+
protected:
virtual void RunIteration(const Goal *unsafe_goal, const Position *position,
aos::Sender<Output>::Builder *output,
@@ -39,6 +42,8 @@
std::shared_ptr<const constants::Values> values_;
frc971::constants::ConstantsFetcher<Constants> constants_fetcher_;
+ EndEffector end_effector_;
+
aos::Alliance alliance_ = aos::Alliance::kInvalid;
DISALLOW_COPY_AND_ASSIGN(Superstructure);
diff --git a/y2023_bot3/control_loops/superstructure/superstructure_goal.fbs b/y2023_bot3/control_loops/superstructure/superstructure_goal.fbs
index 2aad7f1..5659e1f 100644
--- a/y2023_bot3/control_loops/superstructure/superstructure_goal.fbs
+++ b/y2023_bot3/control_loops/superstructure/superstructure_goal.fbs
@@ -11,8 +11,17 @@
SCORE_MID_BACK = 5,
}
+enum RollerGoal: ubyte {
+ IDLE = 0,
+ INTAKE_CUBE = 1,
+ SPIT = 2,
+ SPIT_MID = 3,
+}
+
table Goal {
- pivot_goal:PivotGoal (id: 0);
+ pivot_goal:PivotGoal (id: 0);
+ roller_goal:RollerGoal (id: 1);
+ preloaded_with_cube:bool (id: 2);
}
diff --git a/y2023_bot3/control_loops/superstructure/superstructure_output.fbs b/y2023_bot3/control_loops/superstructure/superstructure_output.fbs
index a109051..652d247 100644
--- a/y2023_bot3/control_loops/superstructure/superstructure_output.fbs
+++ b/y2023_bot3/control_loops/superstructure/superstructure_output.fbs
@@ -3,6 +3,8 @@
table Output {
// Pivot joint voltage
pivot_joint_voltage:double (id: 0);
+ // Roller voltage on the end effector (positive is spitting, negative is sucking)
+ roller_voltage:double (id: 1);
}
root_type Output;
diff --git a/y2023_bot3/control_loops/superstructure/superstructure_position.fbs b/y2023_bot3/control_loops/superstructure/superstructure_position.fbs
index 461ad1e..a629e56 100644
--- a/y2023_bot3/control_loops/superstructure/superstructure_position.fbs
+++ b/y2023_bot3/control_loops/superstructure/superstructure_position.fbs
@@ -1,11 +1,18 @@
include "frc971/control_loops/control_loops.fbs";
include "frc971/can_configuration.fbs";
+include "frc971/control_loops/can_falcon.fbs";
namespace y2023_bot3.control_loops.superstructure;
table Position {
// The position of the pivot joint in radians
pivot_joint_position:frc971.PotAndAbsolutePosition (id: 0);
+
+ // If this is true, the cube beam break is triggered.
+ end_effector_cube_beam_break:bool (id: 1);
+
+ // Roller falcon data
+ roller_falcon:frc971.control_loops.CANFalcon (id: 2);
}
root_type Position;
diff --git a/y2023_bot3/control_loops/superstructure/superstructure_status.fbs b/y2023_bot3/control_loops/superstructure/superstructure_status.fbs
index ca20715..f3df83c 100644
--- a/y2023_bot3/control_loops/superstructure/superstructure_status.fbs
+++ b/y2023_bot3/control_loops/superstructure/superstructure_status.fbs
@@ -3,6 +3,21 @@
namespace y2023_bot3.control_loops.superstructure;
+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,
+ // Waiting for the arm to be at MID shooting goal and then telling the
+ // end effector to spit mid
+ SPITTING_MID = 4
+}
+
table Status {
// All subsystems know their location.
zeroed:bool (id: 0);
@@ -12,6 +27,8 @@
// The current state of the arm.
pivot_joint_state:frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus (id: 2);
+
+ end_effector_state:EndEffectorState (id: 3);
}
root_type Status;