Swerve Drive Optimization Jittering

Raven_Robotics · January 27, 2024, 9:25pm

Our robot drives as intended when the target angle is less than 90 degrees. When near the 90 degree mark (when the optimization would kick in) the wheels jitter rapidly a couple times before righting themselves. Typically, only one or two jitter at a time. Removing the optimize function removes the jitters (but is obviously not ideal). We’ve tried messing with the PID values, and while the problem is less pronounced at very low speeds, it is still present.

We are using a Through Bore Encoder that is plugged into the top of the motor controller. Any help in resolving this issue would be greatly appreciated.

Swerve Module:

  public void setDesiredState(SwerveModuleState desiredState) {
    // Apply chassis angular offset to the desired state.
    SwerveModuleState correctedDesiredState = new SwerveModuleState();
    correctedDesiredState.speedMetersPerSecond = desiredState.speedMetersPerSecond;
    correctedDesiredState.angle = desiredState.angle.plus(Rotation2d.fromRadians(m_chassisAngularOffset));

    // Optimize the reference state to avoid spinning further than 90 degrees.
    SwerveModuleState optimizedDesiredState = SwerveModuleState.optimize(correctedDesiredState,
        new Rotation2d(m_turningEncoder.getPosition()));

    System.out.println("Desired State - " + optimizedDesiredState);

    // Command driving and turning SPARKS MAX towards their respective setpoints.
    m_drivingPIDController.setReference(optimizedDesiredState.speedMetersPerSecond, CANSparkMax.ControlType.kVelocity);
    m_turningPIDController.setReference(optimizedDesiredState.angle.getRadians(), CANSparkMax.ControlType.kPosition);

    m_desiredState = desiredState;
  }

Constants:


    public static final double kDrivingEncoderPositionFactor = (WHEEL_DIAMETER_METERS * Math.PI)
        / DRIVING_MOTOR_REDUCTION; // meters
    public static final double kDrivingEncoderVelocityFactor = ((WHEEL_DIAMETER_METERS * Math.PI)
        / DRIVING_MOTOR_REDUCTION) / 60.0; // meters per second

    public static final double kTurningEncoderPositionFactor = (2 * Math.PI); // radians
    public static final double kTurningEncoderVelocityFactor = (2 * Math.PI) / 60.0; // radians per second

    public static final double kTurningEncoderPositionPIDMinInput = 0; // radians
    public static final double kTurningEncoderPositionPIDMaxInput = 2 * Math.PI; // radians

    public static final double P_MODULE_TURNING_CONTROLLER = 0.1;

    public static final double P_MODULE_DRIVE_CONTROLLER = 0.2;

rtsledge · January 28, 2024, 1:28pm

What type of motor and motor controller combo?

And which type of swerve module?

And could post your constructor code so we can see how everything is being initialized?

Daniel1464 · January 28, 2024, 1:32pm

I would presume that they are using spark maxes + rev encoders because of of the method calls.

rtsledge · January 28, 2024, 1:42pm

agreed, but you know what they say about assumptions……

levyishai · January 28, 2024, 2:30pm

I would assume it is one of the following:

Something wrong with the offsets
The encoder doesn’t return the position in radians
Probably the latter

Kevin_S · January 28, 2024, 2:30pm

My guess is that you are running into problems where either the sensor is not giving you perfect values or if you are manually driving, the joystick is not giving you a perfect value, and that is leading to the shortest path flip flopping.

You could possibly add something that looks at the current velocity of the turning motor, and then bias the code to prefer continuing in one direction when the desired state is approximately 90 degrees.

If you really wanted to be fancy, you could actually take into consideration the angular momentum for the module, and figure out which direction to turn needs the least energy to get to your desired state.

If you wanted to test this theory, you could just always force the motors to turn in the positive direction when near 90 degrees (this might actually fix your stated issue as well).

Raven_Robotics · January 28, 2024, 4:38pm

We are using a RevRobotics ion for our swerve module and a SparkMax as our motor controller.

Here’s the constructor:

/**
   * Constructs a MAXSwerveModule and configures the driving and turning motor,
   * encoder, and PID controller. This configuration is specific to the REV
   * MAXSwerve Module built with NEOs, SPARKS MAX, and a Through Bore
   * Encoder.
   */
  public SwerveModule(int drivingCANId, int turningCANId, double chassisAngularOffset, boolean isReversed) {
    m_drivingSparkMax = new CANSparkMax(drivingCANId, MotorType.kBrushless);
    m_turningSparkMax = new CANSparkMax(turningCANId, MotorType.kBrushless);

   // m_turningSparkMax = new CANSparkMax(0, MotorType.kBrushless);

    // Factory reset, so we get the SPARKS MAX to a known state before configuring
    // them. This is useful in case a SPARK MAX is swapped out.
    m_drivingSparkMax.restoreFactoryDefaults();
    m_turningSparkMax.restoreFactoryDefaults();

    // Setup encoders and PID controllers for the driving and turning SPARKS MAX.
    m_drivingEncoder = m_drivingSparkMax.getEncoder();
    m_turningEncoder = m_turningSparkMax.getAbsoluteEncoder(Type.kDutyCycle);
    m_drivingPIDController = m_drivingSparkMax.getPIDController();
    m_turningPIDController = m_turningSparkMax.getPIDController();
    m_drivingPIDController.setFeedbackDevice(m_drivingEncoder);
    m_turningPIDController.setFeedbackDevice(m_turningEncoder);

    // Apply position and velocity conversion factors for the driving encoder. The
    // native units for position and velocity are rotations and RPM, respectively,
    // but we want meters and meters per second to use with WPILib's swerve APIs.
    m_drivingEncoder.setPositionConversionFactor(ModuleConstants.kDrivingEncoderPositionFactor);
    m_drivingEncoder.setVelocityConversionFactor(ModuleConstants.kDrivingEncoderVelocityFactor);

    // Apply position and velocity conversion factors for the turning encoder. We
    // want these in radians and radians per second to use with WPILib's swerve
    // APIs.
    m_turningEncoder.setPositionConversionFactor(ModuleConstants.kTurningEncoderPositionFactor);
    m_turningEncoder.setVelocityConversionFactor(ModuleConstants.kTurningEncoderVelocityFactor);

    // Invert the turning encoder, since the output shaft rotates in the opposite direction of
    // the steering motor in the MAXSwerve Module.
    if (isReversed)
    {
      m_turningEncoder.setInverted(true);
    }
    
    // Enable PID wrap around for the turning motor. This will allow the PID
    // controller to go through 0 to get to the setpoint i.e. going from 350 degrees
    // to 10 degrees will go through 0 rather than the other direction which is a
    // longer route.
    m_turningPIDController.setPositionPIDWrappingEnabled(true);
    m_turningPIDController.setPositionPIDWrappingMinInput(ModuleConstants.kTurningEncoderPositionPIDMinInput); // Min 0
    m_turningPIDController.setPositionPIDWrappingMaxInput(ModuleConstants.kTurningEncoderPositionPIDMaxInput); // Max 2 * PI
    

    // Set the PID gains for the driving motor. Note these are example gains, and you
    // may need to tune them for your own robot!
    m_drivingPIDController.setP(ModuleConstants.P_MODULE_DRIVE_CONTROLLER);
    m_drivingPIDController.setI(0);
    m_drivingPIDController.setD(0);
    // m_drivingPIDController.setFF(1 / ModuleConstants.kDriveWheelFreeSpeedRps); // FF Value - 0.20875544852568829440720072304831
    m_drivingPIDController.setOutputRange(-1, 1);

    // Set the PID gains for the turning motor. Note these are example gains, and you
    // may need to tune them for your own robot!
    m_turningPIDController.setP(ModuleConstants.P_MODULE_TURNING_CONTROLLER);
    m_turningPIDController.setI(0);
    m_turningPIDController.setD(0);
    // m_turningPIDController.setFF(1 / ModuleConstants.kDriveWheelFreeSpeedRps);
    m_turningPIDController.setOutputRange(-1, 1);

    m_drivingSparkMax.setIdleMode(IdleMode.kCoast);
    m_turningSparkMax.setIdleMode(IdleMode.kCoast);
    m_drivingSparkMax.setSmartCurrentLimit(60);
    m_turningSparkMax.setSmartCurrentLimit(20);

    // Save the SPARK MAX configurations. If a SPARK MAX browns out during
    // operation, it will maintain the above configurations.
    m_drivingSparkMax.burnFlash();
    m_turningSparkMax.burnFlash();

    m_chassisAngularOffset = chassisAngularOffset;
    m_desiredState.angle = new Rotation2d(m_turningEncoder.getPosition());
    m_drivingEncoder.setPosition(0);
  }

rtsledge · January 28, 2024, 6:16pm

Our team started from this code as well and have it working. I compared with ours + original from Rev and found a few differences…

For idle mode (.setIdleMode()) we are using Brake instead of coast.

    m_drivingSparkMax.setIdleMode(IdleMode.kBrake);
    m_turningSparkMax.setIdleMode(IdleMode.kBrake);

We are using a P value for the turning motors of 0.75. I realize you probably reduced from 1 to try to tune the PID. However, I feel like 0.1 is pretty small.
The default periodic status rate for DutyCycle encoders plugged into the sparkmax is 200ms. Your control loop is probably running at 20ms. The data the optimize function is using when you call m_turningEncoder.getPosition() is updating at this rate. Having the data be less stale with optimize might help as well.?.?

This is C++ code below, so you will have to port to Java. We did the following for our status reporting rates. For the duty cycle encoder position, we have it set to report at 15ms.

// see https://github.com/bovlb/frc-tips/blob/main/can-bus/README.md
// see https://docs.revrobotics.com/sparkmax/operating-modes/control-interfaces#periodic-status-frames for details.

   /* Set the spark max periodic frame periods for each of the driving  */
   /* spark max motor controllers.                                      */
   m_drivingSparkMax.SetPeriodicFramePeriod(rev::CANSparkLowLevel::PeriodicFrame::kStatus0, 55);      // Applied *** Output, Faults, Stick Faults, Is Follower
   m_drivingSparkMax.SetPeriodicFramePeriod(rev::CANSparkLowLevel::PeriodicFrame::kStatus1, 15);      // Motor Velocity, Motor Tempature, Motor Voltage, Motor Current
   m_drivingSparkMax.SetPeriodicFramePeriod(rev::CANSparkLowLevel::PeriodicFrame::kStatus2, 15);      // Motor Position
   m_drivingSparkMax.SetPeriodicFramePeriod(rev::CANSparkLowLevel::PeriodicFrame::kStatus3, 65535);   // Analog Sensor Voltage, Velocity, Position
   m_drivingSparkMax.SetPeriodicFramePeriod(rev::CANSparkLowLevel::PeriodicFrame::kStatus4, 65535);   // Alternate Encoder Velocity, Position
   m_drivingSparkMax.SetPeriodicFramePeriod(rev::CANSparkLowLevel::PeriodicFrame::kStatus5, 65535);   // Duty Cycle Absolute Encoder Position, Absolute Angle
   m_drivingSparkMax.SetPeriodicFramePeriod(rev::CANSparkLowLevel::PeriodicFrame::kStatus6, 65565);   // Duty Cycle Absolute Encoder Velocity, Frequency

   /* Set the spark max periodic frame periods for each of the turning  */
   /* spark max motor controllers.                                      */
   m_turningSparkMax.SetPeriodicFramePeriod(rev::CANSparkLowLevel::PeriodicFrame::kStatus0, 55);      // Applied *** Output, Faults, Stick Faults, Is Follower
   m_turningSparkMax.SetPeriodicFramePeriod(rev::CANSparkLowLevel::PeriodicFrame::kStatus1, 65535);   // Motor Velocity, Motor Tempature, Motor Voltage, Motor Current
   m_turningSparkMax.SetPeriodicFramePeriod(rev::CANSparkLowLevel::PeriodicFrame::kStatus2, 65535);   // Motor Position
   m_turningSparkMax.SetPeriodicFramePeriod(rev::CANSparkLowLevel::PeriodicFrame::kStatus3, 65535);   // Analog Sensor Voltage, Velocity, Position
   m_turningSparkMax.SetPeriodicFramePeriod(rev::CANSparkLowLevel::PeriodicFrame::kStatus4, 65535);   // Alternate Encoder Velocity, Position
   m_turningSparkMax.SetPeriodicFramePeriod(rev::CANSparkLowLevel::PeriodicFrame::kStatus5, 15);      // Duty Cycle Absolute Encoder Position, Absolute Angle
   m_turningSparkMax.SetPeriodicFramePeriod(rev::CANSparkLowLevel::PeriodicFrame::kStatus6, 65565);   // Duty Cycle Absolute Encoder Velocity, Frequency

What is your battery voltage when you are testing? When our voltage gets to 11V, we start to get some jitter in our turning. We can see the voltage brown out as low as 9V when we get to this point. I think when we get this jitter is at low voltage we just don’t have enough juice for the way our PID is tuned.

Just some things to try, hopefully one of them helps out!