Initialize turret with AS5600 and set offsets

ut-robomaster/src/drivers/as5600.hpp

@@ -12,7 +12,7 @@ using Board::I2cMaster;
 class As5600 : public Encoder, public modm::I2cDevice<I2cMaster, 1>, public modm::pt::Protothread
 {
 public:
-    As5600() : modm::I2cDevice<I2cMaster, 1>(ADDRESS) {}
+    As5600() : modm::I2cDevice<I2cMaster, 1>(ADDRESS) {};
 
     void update() { run(); }
 
@@ -29,6 +29,7 @@ class As5600 : public Encoder, public modm::I2cDevice<I2cMaster, 1>, public modm
             if (this->wasTransactionSuccessful())
             {
                 angle = (buffer[0] << 8) | buffer[1];
+                online = true;
             }
         }
 
@@ -37,6 +38,8 @@ class As5600 : public Encoder, public modm::I2cDevice<I2cMaster, 1>, public modm
 
     float getAngle() override { return angle / 4096.0f; }
 
+    bool isOnline() override { return online; }
+
 protected:
     enum class Register : uint8_t
     {
@@ -56,6 +59,7 @@ class As5600 : public Encoder, public modm::I2cDevice<I2cMaster, 1>, public modm
     static const uint8_t ADDRESS = 0x36;
     uint16_t angle = 0;
     uint8_t buffer[2];
+    bool online = false;
 };
 
 };  // namespace driver

ut-robomaster/src/drivers/encoder.hpp

@@ -10,5 +10,7 @@ class Encoder
     /// @brief Get current measured angle of the encoder
     /// @return Angle (revs)
     virtual float getAngle() = 0;
+
+    virtual bool isOnline() = 0;
 };
 }  // namespace driver

ut-robomaster/src/robots/hero/hero_constants.hpp

@@ -125,6 +125,8 @@ const MotorConfig
     YAW_L{M3508, MOTOR5, CAN_TURRET, false, "yaw left", {2.5f, 60.0f, 0.0f}, {45.0f, 0.0f, 0.0f}};
 const MotorConfig YAW_R{M3508, MOTOR6, CAN_TURRET, false, "yaw right", {}, {}};
 const MotorConfig PITCH{GM6020, MOTOR7, CAN_TURRET, false, "pitch", PID_VELOCITY_DEFAULT, {}};
+const float YAW_OFFSET = 0;
+const float PITCH_OFFSET = 0;
 
 // Velocities ----------------------------
 

ut-robomaster/src/robots/sentry/sentry_constants.hpp

@@ -140,6 +140,8 @@ const MotorConfig AGITATOR_R{M2006, MOTOR2, CAN_SHOOTER, true, "agitator right",
 // turret
 const MotorConfig YAW{GM6020, MOTOR6, CAN_TURRET, false, "yaw", PID_VELOCITY_DEFAULT, {}};
 const MotorConfig PITCH{GM6020, MOTOR7, CAN_TURRET, false, "pitch", PID_VELOCITY_DEFAULT, {}};
+const float YAW_OFFSET = 0;
+const float PITCH_OFFSET = 0;
 
 // Velocities -------------------------------------
 

ut-robomaster/src/robots/standard/standard_constants.hpp

@@ -123,6 +123,8 @@ const MotorConfig
     YAW_L{M3508, MOTOR5, CAN_TURRET, false, "yaw left", {2.5f, 60.0f, 0.0f}, {45.0f, 0.0f, 0.0f}};
 const MotorConfig YAW_R{M3508, MOTOR6, CAN_TURRET, false, "yaw right", {}, {}};
 const MotorConfig PITCH{GM6020, MOTOR7, CAN_TURRET, false, "pitch", PID_VELOCITY_DEFAULT, {}};
+const float YAW_OFFSET = 0;
+const float PITCH_OFFSET = 0;
 
 // Velocities -------------------------------------
 

ut-robomaster/src/subsystems/turret/double_yaw_motor.cpp

@@ -3,6 +3,8 @@
 #include "tap/algorithms/math_user_utils.hpp"
 #include "tap/motor/dji_motor.hpp"
 
+#include "robots/robot_constants.hpp"
+
 namespace subsystems::turret
 {
 DoubleYawMotor::DoubleYawMotor(
@@ -25,6 +27,7 @@ void DoubleYawMotor::initialize()
 {
     motor1.initialize();
     motor2.initialize();
+    isCalibrated = false;
 }
 
 void DoubleYawMotor::reset()
@@ -37,9 +40,15 @@ void DoubleYawMotor::reset()
 
 void DoubleYawMotor::updateMotorAngle()
 {
-    // float encoderAngle = encoder->getAngle();
-    float encoderAngle = static_cast<float>(motor1.getEncoderUnwrapped()) /
-                         DjiMotor::ENC_RESOLUTION / M3508.gearRatio / 2.0f;
+    if (!isCalibrated && encoder->isOnline())
+    {
+        initialAngle = encoder->getAngle();
+        motor1.resetEncoderValue();
+        isCalibrated = true;
+    }
+    float encoderAngle = (static_cast<float>(motor1.getEncoderUnwrapped()) /
+                          DjiMotor::ENC_RESOLUTION / M3508.gearRatio / 2.0f) +
+                         YAW_OFFSET + initialAngle;
     currentAngle.setValue(encoderAngle);
 }
 

ut-robomaster/src/subsystems/turret/double_yaw_motor.hpp

@@ -43,5 +43,7 @@ class DoubleYawMotor
     motor_controller::Pid positionPid;
     ContiguousFloat setpoint;
     ContiguousFloat currentAngle;
+    bool isCalibrated;
+    float initialAngle;
 };
 }  // namespace subsystems::turret

ut-robomaster/src/subsystems/turret/turret_motor.cpp

@@ -3,6 +3,8 @@
 #include "tap/algorithms/math_user_utils.hpp"
 #include "tap/motor/dji_motor.hpp"
 
+#include "robots/robot_constants.hpp"
+
 namespace subsystems::turret
 {
 TurretMotor::TurretMotor(
@@ -32,8 +34,9 @@ void TurretMotor::updateMotorAngle()
     {
         lastUpdatedEncoderValue = encoderValue;
 
-        unwrappedAngle = static_cast<float>(encoderValue) * M_TWOPI /
-                         static_cast<float>(DjiMotor::ENC_RESOLUTION);
+        unwrappedAngle = (static_cast<float>(encoderValue) * M_TWOPI /
+                          static_cast<float>(DjiMotor::ENC_RESOLUTION)) -
+                         PITCH_OFFSET;
         currentAngle.setValue(unwrappedAngle);
     }
 }