RamseteCommand / Pathweaver Not Turning Left Correctly

[Delaminer]

Hi, I have been trying to get RamseteCommand to work using WPI’s example code and their documentation. My robot I’m testing with is a KOP chassis with 4 NEOs and 4 CAN Spark maxes and an ADXRS450 gyro. I have pretty much just pasted in WPI’s code, just modified encoders and motors to use CANSparkMaxes and CANEncoders, which outputs position/velocity in meters like expected. I published the project to GitHub here. (The project is in Java)

I have two paths: one turning right (going 1.5m forward and 1.5m right) and one turning left( going 1.5m forward and 1.5m left). Turning right works fine, but when I run the turning left path, it first turns right then tries to turn left (which is not what I want). I’v attached an images of the generated paths and videos of them driving:

https://www.chiefdelphi.com/uploads/default/original/3X/4/e/4eecab3945999d00ed0e180c24b0b6d5c1388245.mp4 https://www.chiefdelphi.com/uploads/default/original/3X/9/b/9b494e63b8467fb3b4ff2980033766e1679fa88f.mp4

Here are also the JSON files for the paths:
JustTurnLeft.wpilib.json (8.3 KB) JustTurnRight.wpilib.json (8.4 KB)

I also collected data of the odometry, encoder velocity, position, gyro, and some other values on Shuffleboard(and some other random data from other tests), here are the SBR files: Turnleft recording-13.42.29.sbr (26.0 KB) TurnRight recording-13.41.31.sbr (25.8 KB)
I characterized my robot and am using a Feedforward of kS=0.146, kV=2.17, kA=0.308 and PID of P=2.59, I=0, and D=0 (Track width=0.66m). I’m using a max velocity of 3m/s and acceleration of 3m/s/s.

My code for the autonomous command:
public Command getAutonomousCommand() throws IOException {
var table = NetworkTableInstance.getDefault().getTable(“troubleshooting”);
var leftReference = table.getEntry(“left_reference”);
var leftMeasurement = table.getEntry(“left_measurement”);
var rightReference = table.getEntry(“right_reference”);
var rightMeasurement = table.getEntry(“right_measurement”);

var autoVoltageConstraint =
    new DifferentialDriveVoltageConstraint(
        new SimpleMotorFeedforward(DriveConstants.ksVolts,
                                   DriveConstants.kvVoltSecondsPerMeter,
                                   DriveConstants.kaVoltSecondsSquaredPerMeter),
        DriveConstants.kDriveKinematics,
        10);
var turn = new CentripetalAccelerationConstraint(0.1);

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

String file = "JustTurnRight"; //Change this for which path to use
Trajectory jsonTrajectory = TrajectoryUtil.fromPathweaverJson(Paths.get(
    "/home/lvuser/deploy/output/"+file+".wpilib.json"));
System.out.println("Loaded file "+file+".wpilib.json");
// Paste this variable in
RamseteController disabledRamsete = new RamseteController() {
    @Override
    public ChassisSpeeds calculate(Pose2d currentPose, Pose2d poseRef, double linearVelocityRefMeters,
            double angularVelocityRefRadiansPerSecond) {
        return new ChassisSpeeds(linearVelocityRefMeters, 0.0, angularVelocityRefRadiansPerSecond);
    }
};
PIDController leftController = new PIDController(DriveConstants.kPDriveVel, 0, 0);
PIDController rightController = new PIDController(DriveConstants.kPDriveVel, 0, 0);
RamseteCommand rammand = new RamseteCommand(
    jsonTrajectory, 
    m_robotDrive::getPose,
    new RamseteController(AutoConstants.kRamseteB, AutoConstants.kRamseteZeta),
    DriveConstants.kDriveKinematics,
    (leftVolts, rightVolts) -> {
        m_robotDrive.tankDriveVolts(leftVolts, rightVolts);

        leftMeasurement.setNumber(m_robotDrive.getWheelSpeeds().leftMetersPerSecond);
        leftReference.setNumber(leftController.getSetpoint());

        rightMeasurement.setNumber(m_robotDrive.getWheelSpeeds().rightMetersPerSecond);
        rightReference.setNumber(rightController.getSetpoint());
    },
    m_robotDrive
);
RamseteCommand ramseteCommand = new RamseteCommand(
    jsonTrajectory,
    m_robotDrive::getPose,
    new RamseteController(AutoConstants.kRamseteB, AutoConstants.kRamseteZeta),
    new SimpleMotorFeedforward(DriveConstants.ksVolts,
                               DriveConstants.kvVoltSecondsPerMeter,
                               DriveConstants.kaVoltSecondsSquaredPerMeter),
    DriveConstants.kDriveKinematics,
    m_robotDrive::getWheelSpeeds,
    leftController,
    rightController,
(leftVolts, rightVolts) -> {
    m_robotDrive.tankDriveVolts(leftVolts, rightVolts);leftMeasurement.setNumber(m_robotDrive.getWheelSpeeds().leftMetersPerSecond);
    leftReference.setNumber(leftController.getSetpoint());

rightMeasurement.setNumber(m_robotDrive.getWheelSpeeds().rightMetersPerSecond);
rightReference.setNumber(rightController.getSetpoint());
},
m_robotDrive
);
return ramseteCommand.andThen(() → m_robotDrive.tankDriveVolts(0, 0));
}

I have no idea why TurnLeft does not work when TurnRight does, does anybody know why?
Thanks!

[pietroglyph]

You might take a look at this troubleshooting tutorial that I recently added to frc-docs. The behavior you’re seeing and the fact that your code looks (after a quick read) pretty much fine indicates that this might be a tuning issue. If you don’t go through the entire troubleshooting guide then you should at least check that your wheel P controllers are tracking well (I’m on mobile right now so I can’t see if you already have that data in your Shuffleboard recordings).

[Delaminer]

Sorry I haven’t posted this earlier, but I found out the problem. I was not setting my initial odometry position to the intitial position of the command. On the TurnRight program, it started at (0,0) just like the robot and so it worked fine anyways, but the TurnLeft program didn’t, and so it tried to get back on course (that’s why it moved to the right). I fixed it by adding this in the command:
m_robotDrive.resetOdometry(jsonTrajectory.getInitialPose());