Applying sysid feedforward to an SDS Swerve MK4

Technical Java
, , , ,
#1

Next hurdle. I have Characterized my SDS MK4 chassis byt locking the wheels and using the sysid tool.

image
image1577Ă—936 249 KB

I have code based on the SDS Swerve template. I have added some code to make the drivetrain consume SwerveModuleState][] so it will be useable by the SwerveControllerCommand which produces such an array.

So if I set my gains as indicated in the tool

public static final class TranslationGains {
            public static final double kP = 2.2956;
            public static final double kI = 0;
            public static final double kD = 0;
            public static final double kA = 0.12872;
            public static final double kV = 2.3014;
            public static final double kS = 0.55493;
        }

Create a SimpleMotorFeedForward

    private SimpleMotorFeedforward m_feedForward = new SimpleMotorFeedforward(TranslationGains.kS, TranslationGains.kV, TranslationGains.kA);

To apply the feed forward to my code

    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.normalizeWheelSpeeds(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());
        }
    }

    private double velocityToDriveVolts(double speedMetersPerSecond){
        return speedMetersPerSecond / DrivetrainGeometry.MAX_VELOCITY_METERS_PER_SECOND * MAX_VOLTAGE;
    }

Do I just apply the m_feedForward.calculate to the speedMetersPerSecond in the velocityToDriveVolts or do I pass in the calculated volts first? Or am I completely off track?

2 Likes
SysId gains on SDS MK4i Modules
Need Help Tuning WPILib Based Swerve Autonomous
#2

The feedforward will output in voltage; you add it to the result of your feedback controller and send the sum to the motors.

#3

Using the SwerveControllerCommand the output is the SwerveModuleStates array.
So I pass in the voltage I calculated as a parameter to ff or the speed. I wouldn’t think I would add it in again since the calculate() seems to already add it, I would almost always end up over 12

  public double calculate(double velocity, double acceleration) {
    return ks * Math.signum(velocity) + kv * velocity + ka * acceleration;
  }
2 Likes
#4

No, you do not pass a voltage to the feedforward. Look at the argument names.

#5

So it looks like I just replace my voltage calculation with SimpleMotorFeedforwards calculate(speedMetersPerSecond) and clamp it at 12, Thanks

2 Likes
#6

note that you might also need to implement a deadband, because if your kS is slightly too large the system response will have a discontinuity at v=0 that will cause oscillation.

#7

I think you want to set voltage directly? This is the way we give those values… Using the set voltage command:

 ​public​ ​static​ ​SimpleMotorFeedforward​ driveMotorFeedforward ​=​ ​new​ ​SimpleMotorFeedforward​(​Constants​.​kS, ​Constants​.​kV);
    ​                mDriveMotor​.​setVoltage(driveMotorFeedforward​.​calculate(kState​.​speedMetersPerSecond));
#8

Using the SDS code the motors are abstracted away from the developer. The drive state set is an indirect voltage set of the motor but the best you have without altering their library to expose the motors.

2 Likes
#9

Just finished profiling our SDS Mk3 test chassis robot with the Fast gearbox ( 6.86:1 gear ratio) using Falcon 500s and got very similar numbers to your SDS Mk4 numbers. kS within 4%, kV within 2%, and kP and kA withing 0.5%.

I think you’re running 6.75:1 (L2) versions of the Mk4? So not too different from the 6.86:1 ratio on our Mk3s. Curious if this is a full competition robot or a bare bones test bot like ours is.

This is a relief. It seems like I managed to characterize our swerve bot correctly. Now to get autonomous working…

#10

Yes I am running an l2 6.75:1. It is bare bones, 8 falcons, battery, radio, rio, pdb. Made our of 1x2 30 inches square.

2 Likes
#11

Can you explain why this happens? I noticed something similar with our test swerve drive — oscillation and/or a slow creep in a specific direction without touching the controller. It was eliminated when I increased the controller non-scaled deadband from 0.05 to 0.08, but I’d like to know more in case it happens again. If it’s a case of small voltage inputs causing issues, a scaled deadband like the one in Wpilib MathUtil wouldn’t work, right?

#12

It sounds like a controller issue, since changing your deadband fixed it. I’ve used xbox one controllers that don’t rest at 0 (actually a pretty sizeable gap) and 3rd party 360 controllers that start to randomly drift untouched after some use. It’s pretty spooky when the robot starts slowly spinning in circles while no one is touching the controller. I’m pretty sure the drift there was because the controllers were transported in the pocket of a case that pressed and damaged the sticks.

2 Likes
#13

If kS is large enough to move the robot on its own (if it is ideally-tuned it will be “just” large enough to cancel friction without moving the robot, but the world is not perfect) then you will see creep on small inputs. A deadband causes the output to rigorously be zero (rather than kS or -kS), eliminating creep.

2 Likes
#14

Uh… that seems familiar, I’m going to want to pay a bit more attention to controller care from now on haha.

Thanks for the explanation! Would it be right, then, to say that with a scaled deadband like the WPILib one (which outputs values from 0 to 1), the zero input doesn’t matter since you wouldn’t be inputting that unless you want the robot to move?

#15

I’m unsure what you mean by this. A deadband doesn’t do any scaling; all it does is clamp values below some nominal magnitude to zero.

#16 
/**
 * Returns 0.0 if the given value is within the specified range around zero. The remaining range
 * between the deadband and 1.0 is scaled from 0.0 to 1.0.
 *
 * @param value Value to clip.
 * @param deadband Range around zero.
 * @return The value after the deadband is applied.
 */
public static double applyDeadband(double value, double deadband) {
  if (Math.abs(value) > deadband) {
    if (value > 0.0) {
      return (value - deadband) / (1.0 - deadband);
    } else {
      return (value + deadband) / (1.0 - deadband);
    }
  } else {
    return 0.0;
  }
}

The MathUtil deadband function looks like it scales values above the deadband threshold from the range of (deadband, 1) to (0, 1).

#17

That function probably ought to be renamed, split into two functions, or overloaded to take a scale param (the latter is probably the safest option…). I would not advise using that if you do not actually want to scale your outputs; it looks like it is designed specifically for joysticks.

What you want here is a deadband on the velocity setpoint for the controller.

#18

Oh ok … in effect though, a deadband on the Xbox stick inputs effectively indirectly does that, right? (I didn’t use a scaled deadband on it). It prevents smaller inputs from being given to the motors.

1 Like
#19

Yes.

#20

There’s now a pull request open that will allow applyDeadband to work with an arbitrary scale parameter. The rescaling semantics remain the same, but you can set the rescaling range.

1 Like