Third robot end effector
Signed-off-by: Logan Isaacson <100030671@mvla.net>
Change-Id: Iba7b08c0ecd5828542ef769d7a85cd0f3737f5bf
diff --git a/y2023_bot3/constants.h b/y2023_bot3/constants.h
index a741f3c..500b836 100644
--- a/y2023_bot3/constants.h
+++ b/y2023_bot3/constants.h
@@ -39,6 +39,12 @@
static constexpr double kDrivetrainSupplyCurrentLimit() { return 35.0; }
static constexpr double kDrivetrainStatorCurrentLimit() { return 60.0; }
+ // timeout to ensure code doesn't get stuck after releasing the "intake"
+ // button
+ static constexpr std::chrono::milliseconds kExtraIntakingTime() {
+ return std::chrono::milliseconds{100};
+ }
+
static double DrivetrainEncoderToMeters(int32_t in) {
return ((static_cast<double>(in) /
kDrivetrainEncoderCountsPerRevolution()) *
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;
diff --git a/y2023_bot3/wpilib_interface.cc b/y2023_bot3/wpilib_interface.cc
index 06fe314..2b5a9f4 100644
--- a/y2023_bot3/wpilib_interface.cc
+++ b/y2023_bot3/wpilib_interface.cc
@@ -258,11 +258,18 @@
void set_falcons(std::shared_ptr<Falcon> right_front,
std::shared_ptr<Falcon> right_back,
std::shared_ptr<Falcon> left_front,
- std::shared_ptr<Falcon> left_back) {
+ std::shared_ptr<Falcon> left_back,
+ std::shared_ptr<Falcon> roller_falcon) {
right_front_ = std::move(right_front);
right_back_ = std::move(right_back);
left_front_ = std::move(left_front);
left_back_ = std::move(left_back);
+ roller_falcon = std::move(roller_falcon);
+ }
+
+ std::optional<frc971::control_loops::CANFalconT> roller_falcon_data() {
+ std::unique_lock<aos::stl_mutex> lock(roller_mutex_);
+ return roller_falcon_data_;
}
private:
@@ -312,7 +319,12 @@
aos::Sender<frc971::control_loops::drivetrain::CANPosition>
can_position_sender_;
- std::shared_ptr<Falcon> right_front_, right_back_, left_front_, left_back_;
+ std::shared_ptr<Falcon> right_front_, right_back_, left_front_, left_back_,
+ roller_falcon;
+
+ std::optional<frc971::control_loops::CANFalconT> roller_falcon_data_;
+
+ aos::stl_mutex roller_mutex_;
// Pointer to the timer handler used to modify the wakeup.
::aos::TimerHandler *timer_handler_;
@@ -359,6 +371,27 @@
void RunIteration() override {
superstructure_reading_->Set(true);
+ {
+ auto builder = superstructure_position_sender_.MakeBuilder();
+
+ flatbuffers::Offset<frc971::control_loops::CANFalcon>
+ roller_falcon_offset;
+ auto optional_roller_falcon = can_sensor_reader_->roller_falcon_data();
+ if (optional_roller_falcon.has_value()) {
+ roller_falcon_offset = frc971::control_loops::CANFalcon::Pack(
+ *builder.fbb(), &optional_roller_falcon.value());
+ }
+
+ superstructure::Position::Builder position_builder =
+ builder.MakeBuilder<superstructure::Position>();
+ position_builder.add_end_effector_cube_beam_break(
+ end_effector_cube_beam_break_->Get());
+
+ if (!roller_falcon_offset.IsNull()) {
+ position_builder.add_roller_falcon(roller_falcon_offset);
+ }
+ builder.CheckOk(builder.Send(position_builder.Finish()));
+ }
{
auto builder = drivetrain_position_sender_.MakeBuilder();
@@ -446,7 +479,8 @@
std::array<std::unique_ptr<frc::DigitalInput>, 2> autonomous_modes_;
- std::unique_ptr<frc::DigitalInput> imu_yaw_rate_input_;
+ std::unique_ptr<frc::DigitalInput> imu_yaw_rate_input_,
+ end_effector_cube_beam_break_;
frc971::wpilib::DMAPulseWidthReader imu_yaw_rate_reader_;
@@ -665,6 +699,8 @@
std::make_shared<Falcon>(4, "Drivetrain Bus", &signals_registry);
std::shared_ptr<Falcon> left_back =
std::make_shared<Falcon>(5, "Drivetrain Bus", &signals_registry);
+ std::shared_ptr<Falcon> roller_falcon =
+ std::make_shared<Falcon>(13, "Drivetrain Bus", &signals_registry);
// Thread 3.
::aos::ShmEventLoop can_sensor_reader_event_loop(&config.message());
@@ -673,7 +709,7 @@
std::move(signals_registry));
can_sensor_reader.set_falcons(right_front, right_back, left_front,
- left_back);
+ left_back, roller_falcon);
AddLoop(&can_sensor_reader_event_loop);