commit 7cea8a8782bc8bbb4e27e176f0bd6726648aba1b
author Vinay Siva <100024232@mvla.net> Sat Sep 25 15:06:28 2021 -0700
committer Austin Schuh <austin.linux@gmail.com> Sat Oct 23 18:41:16 2021 -0700
tree d0de4e2ba9423c22a739616250d156d1e664470b
parent 204560965d8c4f44206515a1a45e5db65af6159d

Add wpilib-interface 2021 3rd robot climber

Change-Id: Ic5510c9a003d4cb6ca0a3013c19a84bc622aca45
Signed-off-by: Vinay Siva <100024232@mvla.net>

y2021_bot3/wpilib_interface.cc

diff --git a/y2021_bot3/wpilib_interface.cc b/y2021_bot3/wpilib_interface.cc
index 1ad2f52..1808846 100644
--- a/y2021_bot3/wpilib_interface.cc
+++ b/y2021_bot3/wpilib_interface.cc
@@ -196,17 +196,26 @@
   void set_intake_falcon(
       ::std::unique_ptr<::ctre::phoenix::motorcontrol::can::TalonFX> t) {
     intake_falcon_ = ::std::move(t);
-    ConfigureFalcon(intake_falcon_.get());
+    ConfigureRollerFalcon(intake_falcon_.get());
   }
 
   void set_outtake_falcon(
       ::std::unique_ptr<::ctre::phoenix::motorcontrol::can::TalonFX> t) {
     outtake_falcon_ = ::std::move(t);
-    ConfigureFalcon(outtake_falcon_.get());
+    ConfigureRollerFalcon(outtake_falcon_.get());
+  }
+
+  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 ConfigureFalcon(::ctre::phoenix::motorcontrol::can::TalonFX *falcon) {
+  void ConfigureRollerFalcon(
+      ::ctre::phoenix::motorcontrol::can::TalonFX *falcon) {
     falcon->ConfigSupplyCurrentLimit({true, Values::kRollerSupplyCurrentLimit(),
                                       Values::kRollerSupplyCurrentLimit(), 0});
     falcon->ConfigStatorCurrentLimit({true, Values::kRollerStatorCurrentLimit(),
@@ -223,12 +232,22 @@
   void Write(const superstructure::Output &output) override {
     WriteToFalcon(output.intake_volts(), intake_falcon_.get());
     WriteToFalcon(output.outtake_volts(), outtake_falcon_.get());
+
+    WriteToFalcon(-output.climber_volts(), 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);
+    intake_falcon_->Set(ctre::phoenix::motorcontrol::ControlMode::Disabled, 0);
+    outtake_falcon_->Set(ctre::phoenix::motorcontrol::ControlMode::Disabled, 0);
+  }
 
   ::std::unique_ptr<::ctre::phoenix::motorcontrol::can::TalonFX> intake_falcon_,
       outtake_falcon_;
+
+  ::std::unique_ptr<::ctre::phoenix::motorcontrol::can::TalonFX>
+      climber_falcon_;
 };
 
 class WPILibRobot : public ::frc971::wpilib::WPILibRobotBase {
@@ -274,6 +293,8 @@
         make_unique<::ctre::phoenix::motorcontrol::can::TalonFX>(0));
     superstructure_writer.set_outtake_falcon(
         make_unique<::ctre::phoenix::motorcontrol::can::TalonFX>(1));
+    superstructure_writer.set_climber_falcon(
+        make_unique<::ctre::phoenix::motorcontrol::can::TalonFX>(2));
 
     AddLoop(&output_event_loop);