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/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();