[wombat/utils] use 2rpi * wheel radius for getting the encoder distan…

…ce (CurtinFRC#74) * Standardise encoders * Wpiformat * Updated math for getting the encoder distance * Wpiformat * Wpiformat * Updated encoder constructors to account for wheel radius --------- Co-authored-by: Isaac Turner <spacey_sooty@outlook.com>
spacey-sooty · Jan 10, 2024 · d7b0275 · d7b0275
1 parent 4da5fbd
commit d7b0275
Show file tree

Hide file tree

Showing 2 changed files with 33 additions and 23 deletions.
diff --git a/wombat/src/main/cpp/utils/Encoder.cpp b/wombat/src/main/cpp/utils/Encoder.cpp
@@ -4,10 +4,13 @@
 
 #include "utils/Encoder.h"
 
+#include <cmath>
+
 wom::utils::Encoder::Encoder(double encoderTicksPerRotation, int type,
-                             double reduction)
+                             units::meter_t wheelRadius, double reduction)
     : _reduction(reduction),
       _encoderTicksPerRotation(encoderTicksPerRotation),
+      _wheelRadius(wheelRadius),
       _type(type) {}
 
 double wom::utils::Encoder::GetEncoderTicks() const {
@@ -53,9 +56,7 @@ units::radian_t wom::utils::Encoder::GetEncoderPosition() {
 }
 
 double wom::utils::Encoder::GetEncoderDistance() {
-  return (GetEncoderTicks() /*- _offset.value()*/) * 0.02032;
-  // return (GetEncoderTicks() - _offset.value()) * 2 * 3.141592 * 0.0444754;
-  // return (GetEncoderTicks() - _offset.value());
+  return GetEncoderTicks() * (2 * M_PI) * _wheelRadius.value();
 }
 
 units::radians_per_second_t wom::utils::Encoder::GetEncoderAngularVelocity() {
@@ -75,8 +76,9 @@ double wom::utils::DigitalEncoder::GetEncoderTickVelocity() const {
 }
 
 wom::utils::CANSparkMaxEncoder::CANSparkMaxEncoder(rev::CANSparkMax* controller,
+                                                   units::meter_t wheelRadius,
                                                    double reduction)
-    : wom::utils::Encoder(42, reduction, 2),
+    : wom::utils::Encoder(42, reduction, wheelRadius, 2),
       _encoder(controller->GetEncoder()) {}
 
 double wom::utils::CANSparkMaxEncoder::GetEncoderRawTicks() const {
@@ -96,8 +98,9 @@ double wom::utils::CANSparkMaxEncoder::GetVelocity() const {
 }
 
 wom::utils::TalonFXEncoder::TalonFXEncoder(
-    ctre::phoenix::motorcontrol::can::TalonFX* controller, double reduction)
-    : utils::Encoder(2048, reduction, 0), _controller(controller) {
+    ctre::phoenix::motorcontrol::can::TalonFX* controller,
+    units::meter_t wheelRadius, double reduction)
+    : utils::Encoder(2048, reduction, wheelRadius, 0), _controller(controller) {
   controller->ConfigSelectedFeedbackSensor(
       ctre::phoenix::motorcontrol::TalonFXFeedbackDevice::IntegratedSensor);
 }
@@ -112,8 +115,8 @@ double wom::utils::TalonFXEncoder::GetEncoderTickVelocity() const {
 
 wom::utils::TalonSRXEncoder::TalonSRXEncoder(
     ctre::phoenix::motorcontrol::can::TalonSRX* controller,
-    double ticksPerRotation, double reduction)
-    : wom::utils::Encoder(ticksPerRotation, reduction, 0),
+    double ticksPerRotation, units::meter_t wheelRadius, double reduction)
+    : wom::utils::Encoder(ticksPerRotation, reduction, wheelRadius, 0),
       _controller(controller) {
   controller->ConfigSelectedFeedbackSensor(
       ctre::phoenix::motorcontrol::TalonSRXFeedbackDevice::QuadEncoder);
@@ -128,9 +131,10 @@ double wom::utils::TalonSRXEncoder::GetEncoderTickVelocity() const {
 }
 
 wom::utils::DutyCycleEncoder::DutyCycleEncoder(int channel,
+                                               units::meter_t wheelRadius,
                                                double ticksPerRotation,
                                                double reduction)
-    : wom::utils::Encoder(ticksPerRotation, reduction, 0),
+    : wom::utils::Encoder(ticksPerRotation, reduction, wheelRadius, 0),
       _dutyCycleEncoder(channel) {}
 
 double wom::utils::DutyCycleEncoder::GetEncoderRawTicks() const {
@@ -141,9 +145,10 @@ double wom::utils::DutyCycleEncoder::GetEncoderTickVelocity() const {
   return 0;
 }
 
-wom::utils::CanEncoder::CanEncoder(int deviceNumber, double ticksPerRotation,
-                                   double reduction, std::string name)
-    : wom::utils::Encoder(ticksPerRotation, reduction, 1) {
+wom::utils::CanEncoder::CanEncoder(int deviceNumber, units::meter_t wheelRadius,
+                                   double ticksPerRotation, double reduction,
+                                   std::string name)
+    : wom::utils::Encoder(ticksPerRotation, reduction, wheelRadius, 1) {
   _canEncoder = new CANCoder(deviceNumber, name);
 }
 

diff --git a/wombat/src/main/include/utils/Encoder.h b/wombat/src/main/include/utils/Encoder.h
@@ -10,14 +10,16 @@
 #include <rev/CANSparkMax.h>
 #include <units/angle.h>
 #include <units/angular_velocity.h>
+#include <units/length.h>
 
 #include <string>
 
 namespace wom {
 namespace utils {
 class Encoder {
  public:
-  Encoder(double encoderTicksPerRotation, int type, double reduction = 1.0);
+  Encoder(double encoderTicksPerRotation, int type, units::meter_t wheelRadius,
+          double reduction = 1.0);
 
   virtual double GetEncoderRawTicks() const = 0;
   virtual double GetEncoderTickVelocity() const = 0;  // ticks/s
@@ -42,13 +44,14 @@ class Encoder {
   double _encoderTicksPerRotation;
   units::radian_t _offset = 0_rad;
   int _type = 0;
+  units::meter_t _wheelRadius;
 };
 
 class DigitalEncoder : public Encoder {
  public:
   DigitalEncoder(int channelA, int channelB, double ticksPerRotation,
-                 double reduction = 1)
-      : Encoder(ticksPerRotation, reduction, 0),
+                 units::meter_t wheelRadius, double reduction = 1)
+      : Encoder(ticksPerRotation, reduction, wheelRadius, 0),
         _nativeEncoder(channelA, channelB) {}
 
   double GetEncoderRawTicks() const override;
@@ -65,7 +68,7 @@ class SimCANSparkMaxEncoder;
 class CANSparkMaxEncoder : public Encoder {
  public:
   explicit CANSparkMaxEncoder(rev::CANSparkMax* controller,
-                              double reduction = 1);
+                              units::meter_t wheelRadius, double reduction = 1);
 
   double GetEncoderRawTicks() const override;
   double GetEncoderTickVelocity() const override;
@@ -81,7 +84,7 @@ class CANSparkMaxEncoder : public Encoder {
 class TalonFXEncoder : public Encoder {
  public:
   TalonFXEncoder(ctre::phoenix::motorcontrol::can::TalonFX* controller,
-                 double reduction = 1);
+                 units::meter_t wheelRadius, double reduction = 1);
 
   double GetEncoderRawTicks() const override;
   double GetEncoderTickVelocity() const override;
@@ -93,7 +96,8 @@ class TalonFXEncoder : public Encoder {
 class TalonSRXEncoder : public Encoder {
  public:
   TalonSRXEncoder(ctre::phoenix::motorcontrol::can::TalonSRX* controller,
-                  double ticksPerRotation, double reduction = 1);
+                  double ticksPerRotation, units::meter_t wheelRadius,
+                  double reduction = 1);
 
   double GetEncoderRawTicks() const override;
   double GetEncoderTickVelocity() const override;
@@ -104,8 +108,8 @@ class TalonSRXEncoder : public Encoder {
 
 class DutyCycleEncoder : public Encoder {
  public:
-  DutyCycleEncoder(int channel, double ticksPerRotation = 1,
-                   double reduction = 1);
+  DutyCycleEncoder(int channel, units::meter_t wheelRadius,
+                   double ticksPerRotation = 1, double reduction = 1);
 
   double GetEncoderRawTicks() const override;
   double GetEncoderTickVelocity() const override;
@@ -116,8 +120,9 @@ class DutyCycleEncoder : public Encoder {
 
 class CanEncoder : public Encoder {
  public:
-  CanEncoder(int deviceNumber, double ticksPerRotation = 4095,
-             double reduction = 1, std::string name = "Drivebase");
+  CanEncoder(int deviceNumber, units::meter_t wheelRadius,
+             double ticksPerRotation = 4095, double reduction = 1,
+             std::string name = "Drivebase");
 
   double GetEncoderRawTicks() const override;
   double GetEncoderTickVelocity() const override;