commit 088ca77b6302d9bb104fb4d24cb9bdaa4d282407
author Ravago Jones <ravagojones@gmail.com> Sat Mar 25 22:14:24 2023 -0700
committer Ravago Jones <ravagojones@gmail.com> Sat Mar 25 22:14:24 2023 -0700
tree 0674e04ae71025de1775b76338fd1dbf3838d601
parent 41c93f1218525e6dbb550d4615c2018f5bf44447

Add duty cycle to CANPosition logging

We keep seeing falcons do things that are different from what we
commanded, so log what the falcon is actually doing.

Signed-off-by: Ravago Jones <ravagojones@gmail.com>
Change-Id: Idfeeaf5cf51223f815e9d360d479b64229d33a55

y2023/control_loops/drivetrain/drivetrain_can_position.fbs

diff --git a/y2023/control_loops/drivetrain/drivetrain_can_position.fbs b/y2023/control_loops/drivetrain/drivetrain_can_position.fbs
index 10d7c9a..34fbf12 100644
--- a/y2023/control_loops/drivetrain/drivetrain_can_position.fbs
+++ b/y2023/control_loops/drivetrain/drivetrain_can_position.fbs
@@ -20,6 +20,9 @@
 
   // In meters traveled on the drivetrain
   position:float (id: 5);
+
+  // Nominal range is -1 to 1, but can be -2 to +2
+  duty_cycle: float (id: 6);
 }
 
 // CAN readings from the CAN sensor reader loop

y2023/control_loops/superstructure/superstructure_position.fbs

diff --git a/y2023/control_loops/superstructure/superstructure_position.fbs b/y2023/control_loops/superstructure/superstructure_position.fbs
index 83ca2b6..b09e077 100644
--- a/y2023/control_loops/superstructure/superstructure_position.fbs
+++ b/y2023/control_loops/superstructure/superstructure_position.fbs
@@ -38,6 +38,9 @@
 
   // Raw position
   position:float (id: 5);
+
+  // Nominal range is -1 to 1, but can be -2 to +2
+  duty_cycle:float (id: 6);
 }
 
 table Position {

y2023/wpilib_interface.cc

diff --git a/y2023/wpilib_interface.cc b/y2023/wpilib_interface.cc
index 2f41c91..e8d76c9 100644
--- a/y2023/wpilib_interface.cc
+++ b/y2023/wpilib_interface.cc
@@ -126,7 +126,8 @@
         supply_voltage_(talon_.GetSupplyVoltage()),
         supply_current_(talon_.GetSupplyCurrent()),
         torque_current_(talon_.GetTorqueCurrent()),
-        position_(talon_.GetPosition()) {
+        position_(talon_.GetPosition()),
+        duty_cycle_(talon_.GetDutyCycle()) {
     // device temp is not timesynced so don't add it to the list of signals
     device_temp_.SetUpdateFrequency(kCANUpdateFreqHz);
 
@@ -143,6 +144,9 @@
 
     position_.SetUpdateFrequency(kCANUpdateFreqHz);
     signals->push_back(&position_);
+
+    duty_cycle_.SetUpdateFrequency(kCANUpdateFreqHz);
+    signals->push_back(&duty_cycle_);
   }
 
   void PrintConfigs() {
@@ -226,6 +230,7 @@
     builder.add_supply_voltage(supply_voltage());
     builder.add_supply_current(supply_current());
     builder.add_torque_current(torque_current());
+    builder.add_duty_cycle(duty_cycle());
 
     double invert =
         (inverted_ ==
@@ -244,6 +249,7 @@
   float supply_voltage() const { return supply_voltage_.GetValue().value(); }
   float supply_current() const { return supply_current_.GetValue().value(); }
   float torque_current() const { return torque_current_.GetValue().value(); }
+  float duty_cycle() const { return duty_cycle_.GetValue().value(); }
   float position() const { return position_.GetValue().value(); }
 
   // returns the monotonic timestamp of the latest timesynced reading in the
@@ -269,6 +275,8 @@
   ctre::phoenixpro::StatusSignalValue<units::current::ampere_t> supply_current_,
       torque_current_;
   ctre::phoenixpro::StatusSignalValue<units::angle::turn_t> position_;
+  ctre::phoenixpro::StatusSignalValue<units::dimensionless::scalar_t>
+      duty_cycle_;
 };
 
 class CANSensorReader {
@@ -359,6 +367,7 @@
       roller_falcon_data.supply_voltage = roller_falcon_->supply_voltage();
       roller_falcon_data.device_temp = roller_falcon_->device_temp();
       roller_falcon_data.position = -roller_falcon_->position();
+      roller_falcon_data.duty_cycle = roller_falcon_->duty_cycle();
       roller_falcon_data_ =
           std::make_optional<superstructure::CANFalconT>(roller_falcon_data);
     }