Voltage Compensation on SDS Swerve with Falcons

[Bmongar]

We use SDS MK4 modules with Falcon 500 motors. Last year we did not use voltage compensation as we are learning. I think using voltage compensation on all motors that have any need to achieve or maintain a particular speed throughout the match should have voltage compensation.

I was considering using a saturation limit for voltage of 10.5 (10.5 may be too low, just pulling a number for example) for any closed loop control motor.

Most of the match driving is open loop but is closed on autonomous. In the future we may want to use, depending on the game, some closed loop drive strategies on a swerve drivetrain. It would also make testing autonomous easier as there would be less need to continually swap to fresh batteries as we wouldn’t be basing calculations on 12 volts.

So my question comes as the needed modifications for using the SDS code for voltage compensation. I believe I have it but am mostly looking for feedback confirmation.

There are 2 constants in the SDS Template code that I think are affected.

 public static final double MAX_VOLTAGE = 12.0; 
 public static final int FALCON_500_MAX_RPM = 6380;
 public static final double MAX_VELOCITY_METERS_PER_SECOND = FALCON_500_MAX_RPM / 60.0
                    * SdsModuleConfigurations.MK4_L2.getDriveReduction()
                    * SdsModuleConfigurations.MK4_L2.getWheelDiameter()
                    * Math.PI;

They are used in the feed forward calculation and max speed calculation which is in turn used for the wheel desaturation.

private double velocityToDriveVolts(double speedMetersPerSecond) {
      double ff = m_feedForward.calculate(speedMetersPerSecond);
      return MathUtil.clamp(ff, -MAX_VOLTAGE, MAX_VOLTAGE);
 }

private void updateDriveStates(SwerveModuleState[] desiredStates) {
        if (desiredStates != null) {
            SwerveModuleState frontLeftState = desiredStates[FrontLeftSwerveConstants.STATES_INDEX];
            SwerveModuleState frontRightState = desiredStates[FrontRightSwerveConstants.STATES_INDEX];
            SwerveModuleState backLeftState = desiredStates[BackLeftSwerveConstants.STATES_INDEX];
            SwerveModuleState backRightState = desiredStates[BackRightSwerveConstants.STATES_INDEX];
            SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates,
                    DrivetrainGeometry.MAX_VELOCITY_METERS_PER_SECOND);
            m_frontLeftModule.set(velocityToDriveVolts(frontLeftState.speedMetersPerSecond),
                    frontLeftState.angle.getRadians());
            m_frontRightModule.set(velocityToDriveVolts(frontRightState.speedMetersPerSecond),
                   frontRightState.angle.getRadians());
            m_backLeftModule.set(velocityToDriveVolts(backLeftState.speedMetersPerSecond),
                   backLeftState.angle.getRadians());
            m_backRightModule.set(velocityToDriveVolts(backRightState.speedMetersPerSecond),
                    backRightState.angle.getRadians());

        }

    }

So my thoughts are I should, in Phoenix tuner, set the voltage compensation saturation to 10.5 then put the bot on blocks and in Phoenix tuner set 100% output and see the max wheel speed at that voltage. Then set the MAX_VOLTAGE to 10.5 and the FALCON_500_MAX_RPM value observed in Phoenix. Then set nominal voltage in my module config to 10.5 as well. (or the MAX_VOLTAGE constant to be consistent)

Is this the right way to think about voltage compensation in SDS Swerve?

[SLAB-Mr.Thomas]

I’m sure you know that voltage compensation is wonderful but not magical. If you are requesting speed, “X” in the %VBus -1 to +1 range then you get consistent motor speed

if battery voltage > X * voltage_compensation_setting

I’m guessing what the code is … that there is no PID controller, for example, so what you call feed forward is actually 100% of what you want, then:

If you turn desired motor speed into voltage,
check to “normalize” to the limit the fastest request to what is possible (the clamp),
and then turn that new (possibly lower) voltage into %VBus,
then you have to be sure your feedforward clamp is to the same voltage you told the Talon to use to compensate.
Use the same variable, I trust.

Also, I doubt the motor speed observed on blocks is anywhere near the speed on carpet under the robot weight. The observed speed and the observed voltage at that speed are an inseparable pair that you want to use for your max limit.