Add 2022 climber code
superstructure & wpilib climber code changes made in SuperstructureWriter & Sensor reader
Signed-off-by: Griffin Bui <griffinbui+gerrit@gmail.com>
Change-Id: I5d28db16beb324d8a260e6dc042f54f97c5cdfa3
diff --git a/y2022/control_loops/superstructure/superstructure.cc b/y2022/control_loops/superstructure/superstructure.cc
index e985779..b2c89d1 100644
--- a/y2022/control_loops/superstructure/superstructure.cc
+++ b/y2022/control_loops/superstructure/superstructure.cc
@@ -26,7 +26,7 @@
if (output != nullptr && unsafe_goal != nullptr) {
OutputT output_struct;
-
+ output_struct.climber_voltage = unsafe_goal->climber_speed();
output->CheckOk(output->Send(Output::Pack(*output->fbb(), &output_struct)));
}
@@ -36,8 +36,8 @@
status_builder.add_estopped(false);
if (unsafe_goal != nullptr) {
+ status_builder.add_climber_speed(unsafe_goal->climber_speed());
}
-
(void)status->Send(status_builder.Finish());
}
diff --git a/y2022/control_loops/superstructure/superstructure_goal.fbs b/y2022/control_loops/superstructure/superstructure_goal.fbs
index 439c0fb..3e0679e 100644
--- a/y2022/control_loops/superstructure/superstructure_goal.fbs
+++ b/y2022/control_loops/superstructure/superstructure_goal.fbs
@@ -3,7 +3,9 @@
namespace y2022.control_loops.superstructure;
table Goal {
-
+ // Voltage of the climber falcon
+ // - is down + is up
+ climber_speed:double (id: 0);
}
root_type Goal;
diff --git a/y2022/control_loops/superstructure/superstructure_output.fbs b/y2022/control_loops/superstructure/superstructure_output.fbs
index e95fbe3..78b2c01 100644
--- a/y2022/control_loops/superstructure/superstructure_output.fbs
+++ b/y2022/control_loops/superstructure/superstructure_output.fbs
@@ -1,7 +1,9 @@
namespace y2022.control_loops.superstructure;
table Output {
-
+ // Voltage of the climber falcon
+ // - is down + is up
+ climber_voltage:double (id: 0);
}
root_type Output;
diff --git a/y2022/control_loops/superstructure/superstructure_status.fbs b/y2022/control_loops/superstructure/superstructure_status.fbs
index deff2f6..5f8cdc3 100644
--- a/y2022/control_loops/superstructure/superstructure_status.fbs
+++ b/y2022/control_loops/superstructure/superstructure_status.fbs
@@ -9,6 +9,10 @@
// If true, we have aborted. This is the or of all subsystem estops.
estopped:bool (id: 1);
+
+ // Goal voltage of the climber falcon
+ // - is down + is up
+ climber_speed:double (id: 2);
}
root_type Status;
\ No newline at end of file
diff --git a/y2022/wpilib_interface.cc b/y2022/wpilib_interface.cc
index 5f43b70..f4cbac8 100644
--- a/y2022/wpilib_interface.cc
+++ b/y2022/wpilib_interface.cc
@@ -193,17 +193,33 @@
: ::frc971::wpilib::LoopOutputHandler<superstructure::Output>(
event_loop, "/superstructure") {}
+ void set_climber_falcon(
+ ::std::unique_ptr<::ctre::phoenix::motorcontrol::can::TalonFX> t) {
+ climber_falcon_ = ::std::move(t);
+ climber_falcon_->ConfigSupplyCurrentLimit(
+ {true, Values::kClimberSupplyCurrentLimit(),
+ Values::kClimberSupplyCurrentLimit(), 0});
+ }
+
private:
- void WriteToFalcon(const double voltage,
- ::ctre::phoenix::motorcontrol::can::TalonFX *falcon) {
+ void WriteToFalconCan(const double voltage,
+ ::ctre::phoenix::motorcontrol::can::TalonFX *falcon) {
falcon->Set(
ctre::phoenix::motorcontrol::ControlMode::PercentOutput,
std::clamp(voltage, -kMaxBringupPower, kMaxBringupPower) / 12.0);
}
- void Write(const superstructure::Output & /* output */) override {}
+ void Write(const superstructure::Output &output) override {
+ WriteToFalconCan(output.climber_voltage(), climber_falcon_.get());
+ }
- void Stop() override { AOS_LOG(WARNING, "Superstructure output too old.\n"); }
+ void Stop() override {
+ AOS_LOG(WARNING, "Superstructure output too old.\n");
+ climber_falcon_->Set(ctre::phoenix::motorcontrol::ControlMode::Disabled, 0);
+ }
+
+ ::std::unique_ptr<::ctre::phoenix::motorcontrol::can::TalonFX>
+ climber_falcon_;
};
class WPILibRobot : public ::frc971::wpilib::WPILibRobotBase {
@@ -246,6 +262,9 @@
SuperstructureWriter superstructure_writer(&output_event_loop);
+ superstructure_writer.set_climber_falcon(
+ make_unique<::ctre::phoenix::motorcontrol::can::TalonFX>(0));
+
AddLoop(&output_event_loop);
RunLoops();