Drive train sysid characterization for path following with 4 NEO motors

So I’m trying to get path following to work with our old 2022 robot. I have followed the path following wpilib example.

Unfortunately, when I run it on the robot, the robot just shakes uncontrollably.

here’s my code:
drive subsystem:

package frc.robot.subsystems;

import com.revrobotics.CANSparkMax;
import com.revrobotics.CANSparkMaxLowLevel.MotorType;

import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.kinematics.DifferentialDriveOdometry;
import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds;
import edu.wpi.first.wpilibj.ADXRS450_Gyro;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Constants;

public class DriveTrain extends SubsystemBase {

    private static DriveTrain mInstance; // defining static instance of drivetrain

    private CANSparkMax mLeftLeader; // defining spark max objects for interface with spark max motorcontroller hardware
    private CANSparkMax mRightLeader; // ^^^
    
    private CANSparkMax mLeftFollower; // ^^^
    private CANSparkMax mRightFollower; // ^^^

    private DifferentialDrive mDrive; // defining our drive base

    private ADXRS450_Gyro mGyro; // defining our gyroscope

    private DifferentialDriveOdometry mOdometry;

    public DriveTrain() {
        mLeftLeader = new CANSparkMax(1, MotorType.kBrushless); // initializing motorcontroller object with the CAN ID 1, which was set when we imaged the spark maxes with the proper firmware
        mRightLeader = new CANSparkMax(2, MotorType.kBrushless); // ^^^

        mLeftFollower = new CANSparkMax(3, MotorType.kBrushless); // ^^^
        mRightFollower = new CANSparkMax(4, MotorType.kBrushless); // ^^^

        mDrive = new DifferentialDrive(mLeftLeader, mRightLeader); // initializing our drivebase with two motors - left and right leaders

        mGyro = new ADXRS450_Gyro(); // initializing our gyroscope
        mGyro.calibrate(); // calibrating our gyroscope

        mLeftFollower.follow(mLeftLeader); // setting one NEO to follow the other on the left side
        mRightFollower.follow(mRightLeader); // ^^^

        mDrive.setSafetyEnabled(false); // weird error occurs when this isnt there - probably don't actually do this

        mOdometry = new DifferentialDriveOdometry(mGyro.getRotation2d());
        
        mLeftLeader.getEncoder().setPositionConversionFactor(Constants.POSITION_CONVERSION_FACTOR);
        mRightLeader.getEncoder().setPositionConversionFactor(Constants.POSITION_CONVERSION_FACTOR);
    }

    @Override
    public void periodic() {
    mOdometry.update(
        mGyro.getRotation2d(), mLeftLeader.getEncoder().getPosition(), mRightLeader.getEncoder().getPosition());
    }

    public Pose2d getPose() {
        return mOdometry.getPoseMeters();
    }

    public DifferentialDriveWheelSpeeds getWheelSpeeds() {
        return new DifferentialDriveWheelSpeeds(mLeftLeader.getEncoder().getVelocity(), mRightLeader.getEncoder().getVelocity());
    }

    public void resetOdometry(Pose2d pose) {
        resetEncoders();
        mOdometry.resetPosition(pose, mGyro.getRotation2d());
    }

    public void tankDriveVolts(double leftVolts, double rightVolts) {
        mLeftLeader.setVoltage(leftVolts);
        mRightLeader.setVoltage(rightVolts);
        mDrive.feed();
    }

    public void resetEncoders() {
        mLeftLeader.getEncoder().setPosition(0);
        mRightLeader.getEncoder().setPosition(0);
    }

    public double getAverageEncoderDistance() {
        return (mLeftLeader.getEncoder().getPosition() + mRightLeader.getEncoder().getPosition()) / 2;
    }

    public void drive(double f, double t) { // our drive method
        mDrive.arcadeDrive(f, t); // this method will be what actually converts our joystick values into motor voltages, which are outputted automatically to the NEOs
    }

    public void resetGyro() { // method to reset the gyroscope heading to 0
        mGyro.reset(); // reset gyroscope heading to 0
    }

    public double getHeading() {
        return mGyro.getRotation2d().getDegrees();
    }

    public double getTurnRate() {
        return -mGyro.getRate();
    }

        
    public static DriveTrain getInstance() { // method to get the static instance of the drivetrain
        if (mInstance == null) { // if the drivetrain is not defined
            mInstance = new DriveTrain(); // define it
        }
        return mInstance; // return the drivetrain instance
    }
}

robotcontainer:

// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.

package frc.robot;

import java.util.List;

import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.controller.RamseteController;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.trajectory.Trajectory;
import edu.wpi.first.math.trajectory.TrajectoryConfig;
import edu.wpi.first.math.trajectory.TrajectoryGenerator;
import edu.wpi.first.math.trajectory.constraint.DifferentialDriveVoltageConstraint;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandGroupBase;
import edu.wpi.first.wpilibj2.command.RamseteCommand;
import frc.robot.commands.Drive;
import frc.robot.subsystems.DriveTrain;
/**
 * This class is where the bulk of the robot should be declared. Since Command-based is a
 * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
 * periodic methods (other than the scheduler calls). Instead, the structure of the robot (including
 * subsystems, commands, and button mappings) should be declared here.
 */

public class RobotContainer {
  // The robot's subsystems and commands are defined here...
  private DriveTrain mDriveTrain = DriveTrain.getInstance(); // getting instance of the drivetrain

  private Drive mDrive; // defining drive command object

  SendableChooser<CommandGroupBase> mAutoChooser; // defining autochooser smartdashboard widget

  /** The container for the robot. Contains subsystems, OI devices, and commands. */
  public RobotContainer() {

    SmartDashboard.putData(mAutoChooser);

    // Configure the button bindings
    configureButtonBindings();
  }

  /**
   * Use this method to define your button->command mappings. Buttons can be created by
   * instantiating a {@link GenericHID} or one of its subclasses ({@link
   * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a {@link
   * edu.wpi.first.wpilibj2.command.button.JoystickButton}.
   */
  private void configureButtonBindings() {
    XboxController mController = new XboxController(0);

    mDrive = new Drive(() -> mController.getRightX(), () -> mController.getLeftY());
    
    mDriveTrain.setDefaultCommand(mDrive);
  }

  public Command getAutonomousCommand() {
    // Create a voltage constraint to ensure we don't accelerate too fast
    var autoVoltageConstraint =
        new DifferentialDriveVoltageConstraint(
            new SimpleMotorFeedforward(
                Constants.ksVolts,
                Constants.kvVoltSecondsPerMeter,
                Constants.kaVoltSecondsSquaredPerMeter),
            Constants.kDriveKinematics,
            10);

    // Create config for trajectory
    TrajectoryConfig config =
        new TrajectoryConfig(
                Constants.kMaxSpeedMetersPerSecond,
                Constants.kMaxAccelerationMetersPerSecondSquared)
            // Add kinematics to ensure max speed is actually obeyed
            .setKinematics(Constants.kDriveKinematics)
            // Apply the voltage constraint
            .addConstraint(autoVoltageConstraint);

    // An example trajectory to follow.  All units in meters.
    Trajectory exampleTrajectory =
        TrajectoryGenerator.generateTrajectory(
            // Start at the origin facing the +X direction
            new Pose2d(0, 0, new Rotation2d(0)),
            // Pass through these two interior waypoints, making an 's' curve path
            List.of(new Translation2d(1, 1), new Translation2d(2, -1)),
            // End 3 meters straight ahead of where we started, facing forward
            new Pose2d(3, 0, new Rotation2d(0)),
            // Pass config
            config);

    RamseteCommand ramseteCommand =
        new RamseteCommand(
            exampleTrajectory,
            mDriveTrain::getPose,
            new RamseteController(Constants.kRamseteB, Constants.kRamseteZeta),
            new SimpleMotorFeedforward(
                Constants.ksVolts,
                Constants.kvVoltSecondsPerMeter,
                Constants.kaVoltSecondsSquaredPerMeter),
            Constants.kDriveKinematics,
            mDriveTrain::getWheelSpeeds,
            new PIDController(Constants.kPDriveVel, 0, 0),
            new PIDController(Constants.kPDriveVel, 0, 0),
            mDriveTrain::tankDriveVolts,
            mDriveTrain);

    // Reset odometry to the starting pose of the trajectory.
    mDriveTrain.resetOdometry(exampleTrajectory.getInitialPose());

    // Run path following command, then stop at the end.
    return ramseteCommand.andThen(() -> mDriveTrain.tankDriveVolts(0, 0));
  }
}

constants:

// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.

package frc.robot;

import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
import edu.wpi.first.math.util.Units;

/**
 * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
 * constants. This class should not be used for any other purpose. All constants should be declared
 * globally (i.e. public static). Do not put anything functional in this class.
 *
 * <p>It is advised to statically import this class (or one of its inner classes) wherever the
 * constants are needed, to reduce verbosity.
 */
public final class Constants {

    // robot constants
    public static final double WHEEL_CIRCUMFERENCE = Units.inchesToMeters(6) * Math.PI; // circumference of the wheel
    public static final double GEAR_BOX_RATIO = 10.71; // ratio of the drive gear box
    public static final double POSITION_CONVERSION_FACTOR = 20/1.27; // position conversion factor for drive train

    // trajectory constants
    public static final double kMaxSpeedMetersPerSecond = 3;
    public static final double kMaxAccelerationMetersPerSecondSquared = 3;

    public static final double kRamseteB = 2;
    public static final double kRamseteZeta = 0.7;

    public static final double ksVolts = 0.057532;
    public static final double kvVoltSecondsPerMeter = 2.9886;
    public static final double kaVoltSecondsSquaredPerMeter = 0.71149;

    public static final double kPDriveVel = 4.1505;

    public static final double kTrackwidthMeters = 0.69;
    public static final DifferentialDriveKinematics kDriveKinematics = new DifferentialDriveKinematics(kTrackwidthMeters);
}

this is my SYSID configuration:

I’m not sure what I’m doing wrong. Is my SYSID configuration correct. We’re using the NEO integrated encoders. If anyone has come across this issue can let me know how to fix it that would be great!!

Make sure you enter a realistic measurement delay into SysId. The internal NEO encoders have a substantial filter on their measured velocity.

What do you mean by measurement delay?

In the feedback analysis there are different gain presets.
In order to accurately configure the P value, the system needs to be aware of the measurement delay of the encoder (see feedback analysis there’s Measurement Delay at the bottom).
I believe there is a Spark Max Brushless Preset that gets the NEO encoder delay for you.

1 Like

Do I need to add anything in my code to account for this or is it just SYSid?

SysId will adjust the reported feedback gain for you. Just use the gains it recommends once you have specified the correct measurement delay.

1 Like

So, I’ve implemented that solution. Sadly, it still shakes uncontrollably. It sort of looks like it is following the path though. Is it possible my conversion factors are incorrect?

public static final double POSITION_CONVERSION_FACTOR = ((1/GEAR_BOX_RATIO) * (2*Math.PI*WHEEL_DIAMETER));
public static final double VELOCITY_CONVERSION_FACTOR = (1/GEAR_BOX_RATIO) * (2*Math.PI*WHEEL_DIAMETER)*(1/60);

this is how I implement them in my drive subsystem:

mLeftLeader.getEncoder().setPositionConversionFactor(Constants.POSITION_CONVERSION_FACTOR);
mRightLeader.getEncoder().setPositionConversionFactor(Constants.POSITION_CONVERSION_FACTOR);
mLeftLeader.getEncoder().setVelocityConversionFactor(Constants.VELOCITY_CONVERSION_FACTOR);
mRightLeader.getEncoder().setVelocityConversionFactor(Constants.VELOCITY_CONVERSION_FACTOR);

You probably want all three 1’s to be be 1.0. Java’s integer division may not be doing what you expect.

3 Likes

So it’s following the path now but it is going way too far than it should. I have it set to go 2 meters and It goes 6 meters. The motors are in brake mode. Are my conversions wrong?

Yep. The equation for the circumference of a wheel is 2 pi radius not 2 pi diameter. Change to pi * diameter.

1 Like

Oh goodness. I can’t believe I missed that! Thanks so much!

I finally got it working! I wanted to thank everyone so much for the help!

1 Like

This topic was automatically closed 365 days after the last reply. New replies are no longer allowed.