commit 62e3d3271de65fb545eca4686f54bb90da494403
author Vinay Siva <100024232@mvla.net> Sat Aug 14 17:37:51 2021 -0700
committer Vinay Siva <100024232@mvla.net> Sat Sep 04 15:08:54 2021 -0700
tree 09a553d64c27b567572385a29bd6fbf279673a73
parent 2bdca1866e655cea190b1ca79b9175bb3710afff

Process output in 3rd robot wpilib-interface

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

y2021_bot3/constants.h

diff --git a/y2021_bot3/constants.h b/y2021_bot3/constants.h
index 02c08bc..fd6ab72 100644
--- a/y2021_bot3/constants.h
+++ b/y2021_bot3/constants.h
@@ -27,6 +27,8 @@
            constants::Values::kDrivetrainEncoderRatio() *
            kDrivetrainEncoderCountsPerRevolution();
   }
+  static constexpr double kRollerSupplyCurrentLimit() { return 30.0; }
+  static constexpr double kRollerStatorCurrentLimit() { return 40.0; }
 };
 
 // Creates (once) a Values instance for ::aos::network::GetTeamNumber() and

y2021_bot3/wpilib_interface.cc

diff --git a/y2021_bot3/wpilib_interface.cc b/y2021_bot3/wpilib_interface.cc
index 3b6b691..1ad2f52 100644
--- a/y2021_bot3/wpilib_interface.cc
+++ b/y2021_bot3/wpilib_interface.cc
@@ -31,6 +31,7 @@
 #include "aos/util/log_interval.h"
 #include "aos/util/phased_loop.h"
 #include "aos/util/wrapping_counter.h"
+#include "ctre/phoenix/motorcontrol/can/TalonFX.h"
 #include "ctre/phoenix/motorcontrol/can/TalonSRX.h"
 #include "frc971/autonomous/auto_mode_generated.h"
 #include "frc971/control_loops/drivetrain/drivetrain_position_generated.h"
@@ -192,10 +193,42 @@
       : ::frc971::wpilib::LoopOutputHandler<superstructure::Output>(
             event_loop, "/superstructure") {}
 
+  void set_intake_falcon(
+      ::std::unique_ptr<::ctre::phoenix::motorcontrol::can::TalonFX> t) {
+    intake_falcon_ = ::std::move(t);
+    ConfigureFalcon(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());
+  }
+
  private:
-  void Write(const superstructure::Output & /*output*/) override {}
+  void ConfigureFalcon(::ctre::phoenix::motorcontrol::can::TalonFX *falcon) {
+    falcon->ConfigSupplyCurrentLimit({true, Values::kRollerSupplyCurrentLimit(),
+                                      Values::kRollerSupplyCurrentLimit(), 0});
+    falcon->ConfigStatorCurrentLimit({true, Values::kRollerStatorCurrentLimit(),
+                                      Values::kRollerStatorCurrentLimit(), 0});
+  }
+
+  void WriteToFalcon(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 {
+    WriteToFalcon(output.intake_volts(), intake_falcon_.get());
+    WriteToFalcon(output.outtake_volts(), outtake_falcon_.get());
+  }
 
   void Stop() override { AOS_LOG(WARNING, "Superstructure output too old.\n"); }
+
+  ::std::unique_ptr<::ctre::phoenix::motorcontrol::can::TalonFX> intake_falcon_,
+      outtake_falcon_;
 };
 
 class WPILibRobot : public ::frc971::wpilib::WPILibRobotBase {
@@ -237,6 +270,10 @@
         ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(1)), false);
 
     SuperstructureWriter superstructure_writer(&output_event_loop);
+    superstructure_writer.set_intake_falcon(
+        make_unique<::ctre::phoenix::motorcontrol::can::TalonFX>(0));
+    superstructure_writer.set_outtake_falcon(
+        make_unique<::ctre::phoenix::motorcontrol::can::TalonFX>(1));
 
     AddLoop(&output_event_loop);