commit 322ebb9eba78ba257f320e8651b1a53fa85c90c8
author Jacob Ismael <jacob.ismael@gmail.com> Wed Feb 09 20:12:47 2022 -0800
committer Jacob Ismael <jacob.ismael@gmail.com> Wed Feb 16 19:20:59 2022 -0800
tree 2d49f355e1e4aa66f405cfd09275cf78b1f459f5
parent 72280dfd031f7e101efda3cdd94d7d5a86bbe875

Added set functions for turret and catapult falcons

Signed-off-by: Jacob Ismael <jacob.ismael@gmail.com>
Change-Id: I209a25013b812f0c1b6efed5e165b2608d5a8ce5

y2022/wpilib_interface.cc

diff --git a/y2022/wpilib_interface.cc b/y2022/wpilib_interface.cc
index 0ee1e92..9586c25 100644
--- a/y2022/wpilib_interface.cc
+++ b/y2022/wpilib_interface.cc
@@ -189,6 +189,18 @@
          Values::kClimberSupplyCurrentLimit(), 0});
   }
 
+  void set_turret_falcon(::std::unique_ptr<::frc::TalonFX> t) {
+    turret_falcon_ = ::std::move(t);
+  }
+
+  void set_catapult_falcon_1(::std::unique_ptr<::frc::TalonFX> t) {
+    catapult_falcon_1_ = ::std::move(t);
+  }
+
+  void set_catapult_falcon_2(::std::unique_ptr<::frc::TalonFX> t) {
+    catapult_falcon_2_ = ::std::move(t);
+  }
+
  private:
   void WriteToFalconCan(const double voltage,
                         ::ctre::phoenix::motorcontrol::can::TalonFX *falcon) {
@@ -199,15 +211,32 @@
 
   void Write(const superstructure::Output &output) override {
     WriteToFalconCan(output.climber_voltage(), climber_falcon_.get());
+    catapult_falcon_1_->SetSpeed(std::clamp(output.catapult_voltage(),
+                                            -kMaxBringupPower,
+                                            kMaxBringupPower) /
+                                 12.0);
+    catapult_falcon_2_->SetSpeed(std::clamp(output.catapult_voltage(),
+                                            -kMaxBringupPower,
+                                            kMaxBringupPower) /
+                                 12.0);
+    turret_falcon_->SetSpeed(std::clamp(output.turret_voltage(),
+                                        -kMaxBringupPower, kMaxBringupPower) /
+                             12.0);
   }
 
   void Stop() override {
     AOS_LOG(WARNING, "Superstructure output too old.\n");
     climber_falcon_->Set(ctre::phoenix::motorcontrol::ControlMode::Disabled, 0);
+    catapult_falcon_1_->SetDisabled();
+    catapult_falcon_2_->SetDisabled();
+    turret_falcon_->SetDisabled();
   }
 
   ::std::unique_ptr<::ctre::phoenix::motorcontrol::can::TalonFX>
       climber_falcon_;
+
+  ::std::unique_ptr<::frc::TalonFX> turret_falcon_, catapult_falcon_1_,
+      catapult_falcon_2_;
 };
 
 class WPILibRobot : public ::frc971::wpilib::WPILibRobotBase {
@@ -252,6 +281,9 @@
 
     superstructure_writer.set_climber_falcon(
         make_unique<::ctre::phoenix::motorcontrol::can::TalonFX>(0));
+    superstructure_writer.set_turret_falcon(make_unique<::frc::TalonFX>(2));
+    superstructure_writer.set_catapult_falcon_1(make_unique<::frc::TalonFX>(3));
+    superstructure_writer.set_catapult_falcon_2(make_unique<::frc::TalonFX>(4));
 
     AddLoop(&output_event_loop);