dtyuidrthgfuhfjg

Author: Suhas

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

@@ -44,7 +44,7 @@ constexpr float CAMERA_X_OFFSET = 0.0f;    // horizontal offset of main camera l
 constexpr float PITCH_MIN = -0.3349f;      // rad
 constexpr float PITCH_MAX = 0.3534f;       // rad
 
-static constexpr float YAW_REDUCTION = 1.0f;
+static constexpr float YAW_REDUCTION = 2.005459491f;
 static constexpr float PITCH_REDUCTION = 1.0f;
 
 // Tuning Constants -----------------------------------

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

@@ -34,8 +34,9 @@ void DoubleYawMotor::reset()
 
 void DoubleYawMotor::updateMotorAngle()
 {
-    float encoderAngle = static_cast<float>(motor1.getEncoder()->getPosition().getWrappedValue()) /
-                         DjiMotorEncoder::ENC_RESOLUTION / M3508.gearRatio / YAW_REDUCTION;
+    float encoderAngle =
+        static_cast<float>(motor1.getInternalEncoder().getEncoder().getUnwrappedValue()) /
+        DjiMotorEncoder::ENC_RESOLUTION / M3508.gearRatio / YAW_REDUCTION;
     currentAngle.setWrappedValue(encoderAngle);
 }
 
@@ -47,8 +48,8 @@ void DoubleYawMotor::setAngle(float desiredAngle, float dt)
                               .minDifference(setpoint.getWrappedValue());
 
     // account for chassis rotation
-    float disturbance = drivers->bmi088.getGz() / 360.0f;  // rev / s
-    disturbance *= 2.0f;                                   // seems to help?
+    float disturbance = drivers->bmi088.getGz() / 2.0f / PI;  // rev / s
+    disturbance *= 2.0f;                                      // seems to help?
 
     float targetVelocity = positionPid.update(positionError, dt, false) - disturbance;
     setVelocity(targetVelocity, dt);

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

@@ -28,7 +28,7 @@ void TurretMotor::reset()
 void TurretMotor::updateMotorAngle()
 {
     // uint16_t encoderValue = motor.getEncoderWrapped();
-    uint16_t encoderValue = motor.getEncoder()->getPosition().getWrappedValue();
+    uint16_t encoderValue = motor.getInternalEncoder().getEncoder().getUnwrappedValue();
     if (lastUpdatedEncoderValue != encoderValue)
     {
         lastUpdatedEncoderValue = encoderValue;

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

@@ -32,20 +32,6 @@ void TurretSubsystem::initialize()
 
 void TurretSubsystem::refresh()
 {
-    // yaw.setOutput(0.5f);
-    // pitch.motor.setDesiredOutput(M3508.maxOutput * 0.5);
-    // who needs hardware checks?
-    // setAmputated(!hardwareOk());
-
-#if defined(TARGET_HERO)
-    Remote* remote = &drivers->remote;
-    float h = remote->getChannel(Remote::Channel::LEFT_HORIZONTAL);
-    float v = remote->getChannel(Remote::Channel::LEFT_VERTICAL);
-    yaw.setOutput(h);
-    pitch.motor.setDesiredOutput(GM6020.maxOutput * v);
-    return;
-#endif
-
     yaw.updateMotorAngle();
     pitch.updateMotorAngle();
 
@@ -54,7 +40,11 @@ void TurretSubsystem::refresh()
 
     if (!isCalibrated && yawEncoder.isOnline())
     {
+#if defined(TARGET_HERO)
+        baseYaw = -yaw.getAngle();
+#else
         baseYaw = yawEncoder.getAngle() - YAW_OFFSET - yaw.getAngle();
+#endif
         isCalibrated = true;
 
         setTargetWorldAngles(getCurrentLocalYaw() + getChassisYaw(), getCurrentLocalPitch());
@@ -94,7 +84,7 @@ void TurretSubsystem::setTargetWorldAngles(float yaw, float pitch)
     targetWorldPitch = modm::min(modm::max(pitch, PITCH_MIN), PITCH_MAX);
 }
 
-float TurretSubsystem::getChassisYaw() { return modm::toRadian(drivers->bmi088.getYaw()); }
+float TurretSubsystem::getChassisYaw() { return drivers->bmi088.getYaw(); }
 
 float TurretSubsystem::getTargetLocalYaw() { return targetWorldYaw - getChassisYaw(); }
 

ut-robomaster/src/utils/motors/motor_controller.cpp

@@ -50,7 +50,7 @@ bool MotorController::isOnline() { return motor.isMotorOnline(); }
 
 float MotorController::measurePosition()
 {
-    int64_t encoderVal = motor.getEncoder()->getPosition().getUnwrappedValue();
+    int64_t encoderVal = motor.getInternalEncoder().getEncoder().getUnwrappedValue();
     float units = static_cast<float>(encoderVal) / DjiMotorEncoder::ENC_RESOLUTION;
     float turns = units / constants.gearRatio;  // revs
     return turns;