Merge pull request #133 from ut-ras/stable-beyblade

Stabilize beyblade (fix wobbling)

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

@@ -121,8 +121,9 @@ const MotorConfig AGITATOR{M3508, MOTOR1, CAN_SHOOTER, false, "agitator", PID_AG
 const MotorConfig FEEDER{M2006, MOTOR2, CAN_SHOOTER, false, "feeder", PID_FEEDER, {}};
 
 // turret
-const MotorConfig YAW_L{M3508, MOTOR5, CAN_TURRET, false, "yaw left", PID_VELOCITY_DEFAULT, {}};
-const MotorConfig YAW_R{M3508, MOTOR6, CAN_TURRET, false, "yaw right", PID_VELOCITY_DEFAULT, {}};
+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, {}};
 
 // Velocities ----------------------------

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

@@ -117,8 +117,11 @@ const MotorConfig
 const MotorConfig AGITATOR{M3508, MOTOR1, CAN_SHOOTER, false, "agitator", PID_AGITATOR, {}};
 
 // turret
-const MotorConfig YAW_L{M3508, MOTOR5, CAN_TURRET, false, "yaw left", PID_VELOCITY_DEFAULT, {}};
-const MotorConfig YAW_R{M3508, MOTOR6, CAN_TURRET, false, "yaw right", PID_VELOCITY_DEFAULT, {}};
+// {30.0f, 0.0f, 0.0f}
+// {60.0f, 0.0f, 1.0f}
+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, {}};
 
 // Velocities -------------------------------------

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

@@ -9,15 +9,15 @@ DoubleYawMotor::DoubleYawMotor(
     src::Drivers *drivers,
     MotorConfig motor1,
     MotorConfig motor2,
-    Encoder *encoder,
-    const SmoothPidConfig &pidConfig)
+    Encoder *encoder)
     : drivers(drivers),
       motor1(drivers, motor1.id, motor1.canBus, motor1.inverted, motor1.name),
       motor2(drivers, motor2.id, motor2.canBus, motor2.inverted, motor2.name),
       encoder(encoder),
-      pid(pidConfig),
-      setpoint(0.0f, 0.0f, M_TWOPI),
-      currentAngle(0.0f, 0.0f, M_TWOPI)
+      velocityPid(motor1.velocityPidConstants),
+      positionPid(motor1.positionPidConstants),
+      setpoint(0.0f, 0.0f, 1.0f),
+      currentAngle(0.0f, 0.0f, 1.0f)
 {
 }
 
@@ -31,31 +31,56 @@ void DoubleYawMotor::reset()
 {
     motor2.setDesiredOutput(0);
     motor1.setDesiredOutput(0);
-    pid.reset();
+    velocityPid.reset();
+    positionPid.reset();
 }
 
 void DoubleYawMotor::updateMotorAngle()
 {
-    float encoderAngle = encoder->getAngle() * M_TWOPI;
+    // float encoderAngle = encoder->getAngle();
+    float encoderAngle = static_cast<float>(motor1.getEncoderUnwrapped()) /
+                         DjiMotor::ENC_RESOLUTION / M3508.gearRatio / 2.0f;
     currentAngle.setValue(encoderAngle);
 }
 
 void DoubleYawMotor::setAngle(float desiredAngle, float dt)
 {
-    setpoint.setValue(desiredAngle);
+    setpoint.setValue(desiredAngle / M_TWOPI);
 
-    float positionControllerError =
-        ContiguousFloat(currentAngle.getValue(), 0, M_TWOPI).difference(setpoint.getValue());
-    float rpm1 = motor1.getShaftRPM();                        // rev / m
-    float rpm2 = motor2.getShaftRPM();                        // rev / m
-    float avgRps = (rpm1 + rpm2) / 2.0f * (M_TWOPI / 60.0f);  // rad / s
-    float output = pid.runController(positionControllerError, avgRps, dt);
+    float positionError =
+        ContiguousFloat(currentAngle.getValue(), 0, 1.0f).difference(setpoint.getValue());
 
-    motor1.setDesiredOutput(output);
-    motor2.setDesiredOutput(output);
+    // account for chassis rotation
+    float disturbance = drivers->bmi088.getGz() / 360.0f;  // rev / s
+    disturbance *= 2.0f;                                   // seems to help?
+
+    float targetVelocity = positionPid.update(positionError, dt, false) - disturbance;
+    setVelocity(targetVelocity, dt);
+}
+
+float DoubleYawMotor::getAngle() { return currentAngle.getValue() * M_TWOPI; }
+
+void DoubleYawMotor::setVelocity(float velocity, float dt)
+{
+    float output = velocityPid.update(velocity - getCurrentVelocity(), dt, true);
+    setOutput(output);
+}
+
+float DoubleYawMotor::getCurrentVelocity()
+{
+    float rpm1 = motor1.getShaftRPM();                                       // rev / m
+    float rpm2 = motor2.getShaftRPM();                                       // rev / m
+    float currentVelocity = (rpm1 + rpm2) / 2.0f / 60.0f / M3508.gearRatio;  // rev / s
+    return currentVelocity;
+}
+
+void DoubleYawMotor::setOutput(float output)
+{
+    motor1.setDesiredOutput(output * M3508.maxOutput);
+    motor2.setDesiredOutput(output * M3508.maxOutput);
 }
 
-float DoubleYawMotor::getAngle() { return currentAngle.getValue(); }
+float DoubleYawMotor::getOutput() { return motor1.getOutputDesired() / M3508.maxOutput; }
 
 float DoubleYawMotor::getSetpoint() { return setpoint.getValue(); }
 bool DoubleYawMotor::isOnline() { return motor1.isMotorOnline() && motor2.isMotorOnline(); }

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

@@ -6,6 +6,7 @@
 
 #include "drivers/encoder.hpp"
 #include "utils/motors/motor_constants.hpp"
+#include "utils/motors/pid.hpp"
 
 #include "drivers.hpp"
 
@@ -19,18 +20,17 @@ using driver::Encoder;
 class DoubleYawMotor
 {
 public:
-    DoubleYawMotor(
-        src::Drivers *drivers,
-        MotorConfig motor1,
-        MotorConfig motor2,
-        Encoder *encoder,
-        const SmoothPidConfig &pidConfig);
+    DoubleYawMotor(src::Drivers *drivers, MotorConfig motor1, MotorConfig motor2, Encoder *encoder);
 
     void initialize();
     void reset();
     void updateMotorAngle();
     void setAngle(float desiredAngle, float dt);
     float getAngle();
+    void setVelocity(float velocity, float dt);
+    float getCurrentVelocity();
+    void setOutput(float output);
+    float getOutput();
     float getSetpoint();
     bool isOnline();
 
@@ -39,7 +39,8 @@ class DoubleYawMotor
     DjiMotor motor1;
     DjiMotor motor2;
     Encoder *encoder;
-    SmoothPid pid;
+    motor_controller::Pid velocityPid;
+    motor_controller::Pid positionPid;
     ContiguousFloat setpoint;
     ContiguousFloat currentAngle;
 };

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

@@ -18,7 +18,7 @@ TurretSubsystem::TurretSubsystem(src::Drivers* drivers)
       drivers(drivers),
 #if defined(TARGET_STANDARD) || defined(TARGET_HERO)
       yawEncoder(),
-      yaw(drivers, YAW_L, YAW_R, &yawEncoder, YAW_PID_CONFIG),
+      yaw(drivers, YAW_L, YAW_R, &yawEncoder),
 #else
       yaw(drivers, YAW, YAW_PID_CONFIG),
 #endif