From d7b0275673901ebbf043bb526738f82a8049f219 Mon Sep 17 00:00:00 2001 From: Paul Hodges <111325206+Superbro525Alt@users.noreply.github.com> Date: Wed, 10 Jan 2024 11:50:37 +0800 Subject: [PATCH] [wombat/utils] use 2rpi * wheel radius for getting the encoder distance (#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 --- wombat/src/main/cpp/utils/Encoder.cpp | 31 ++++++++++++++----------- wombat/src/main/include/utils/Encoder.h | 25 ++++++++++++-------- 2 files changed, 33 insertions(+), 23 deletions(-) diff --git a/wombat/src/main/cpp/utils/Encoder.cpp b/wombat/src/main/cpp/utils/Encoder.cpp index 4e403361..895b74cb 100644 --- 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 + 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 index 50c1e21a..6cd545f4 100644 --- a/wombat/src/main/include/utils/Encoder.h +++ b/wombat/src/main/include/utils/Encoder.h @@ -10,6 +10,7 @@ #include #include #include +#include #include @@ -17,7 +18,8 @@ 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;