Pathweaver and Ramsete issues

My first attempts at using Ramesete controller and trajectory following with my robot I have seem to come into some issues. I used robot characterization to obtain all values and all values seemed perfect based off of what the encoder values said. When trying to run the example trajectory on wpilib documentation this is what our robot does. Any ideas?

Auto Constants

    public static final double ksVolts = .469;
    public static final double kvVoltSecondsPerMeter = .469;
    public static final double kaVoltSecondsSquaredPerMeter = .0263;

    public static final double kPDriveVel = .989;

    public static final double kMaxSpeed = 1;
    public static final double kMaxAcceleration = 1;

    public static final double kRameseteB = 2;
    public static final double kRameseteZeta = .7;
    public static final double trackWidthMeters = .7239;

Auto Command

var autoVoltageConstraint =
        new DifferentialDriveVoltageConstraint(
                new SimpleMotorFeedforward(
                        Constants.AutoConstants.ksVolts,
                        Constants.AutoConstants.kvVoltSecondsPerMeter,
                        Constants.AutoConstants.kaVoltSecondsSquaredPerMeter
                ),
                Constants.AutoConstants.kDriveKinematics,
                10
        );

TrajectoryConfig config =
        new TrajectoryConfig(
                Constants.AutoConstants.kMaxSpeed,
                Constants.AutoConstants.kMaxAcceleration
        ).setKinematics(Constants.AutoConstants.kDriveKinematics)
        .addConstraint(autoVoltageConstraint);

String searchAJson = "paths/SearchA.wpilib.json";

Trajectory searchATrajectory = new Trajectory();


try {
  Path galacticSearchAPath = Filesystem.getDeployDirectory().toPath().resolve(searchAJson);
  searchATrajectory = TrajectoryUtil.fromPathweaverJson(galacticSearchAPath);
} catch (IOException e) {
  DriverStation.reportError("Unable to open trajectory: " + searchAJson, e.getStackTrace());
}

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 initialCommand = new RamseteCommand(
        exampleTrajectory,
        drive::getPose,
        new RamseteController(Constants.AutoConstants.kRameseteB, Constants.AutoConstants.kRameseteZeta),
        new SimpleMotorFeedforward(
                Constants.AutoConstants.ksVolts,
                Constants.AutoConstants.kvVoltSecondsPerMeter,
                Constants.AutoConstants.kaVoltSecondsSquaredPerMeter
        ),
        Constants.AutoConstants.kDriveKinematics,
        drive::getSpeeds,
        new PIDController(Constants.AutoConstants.kPDriveVel, 0, 0),
        new PIDController(Constants.AutoConstants.kPDriveVel, 0, 0),
        drive::tankDriveVolts,
        drive
);

drive.resetSensors(exampleTrajectory.getInitialPose());

return initialCommand.andThen(() -> drive.tankDriveVolts(0, 0));

Video

Troubleshooting — FIRST Robotics Competition documentation (wpilib.org)

Follow all steps here to help.

Is your gyro inversions correct? That video looks like your gyro inversions are bad

Link to my teams current code

@Daltz3

public double getHeading() {
    return Math.IEEEremainder(gyro.getAngle(), 360) * (true ? -1.0 : 1.0);
} 

My current code for getting heading from Gyro. Any ideas on what could be causing it to mess up? The number increments up going counter clockwise and goes down clockwise.

Did you test a simpler trajectory first? Always start with a trajectory that drives straight forward 1 meter. We use that to make sure all the motors and gyros are turning the correct directions. You can measure how far the robot actually drove and make sure the gearing and wheel circumference values are correct as well. Keep in mind if your gyro is incorrectly reversed and your left/right drivetrain is incorrectly inverted your robot might be able to drive straight but then suddenly spin in circles if you try to turn. (don’t ask how I know this).

Spinning in place might be the gyro being reversed, or maybe one or the other side of the drivetrain isn’t correctly inverted. Some IMUs are “reversed” (NAVX and a few others) CTRE Pigeons are not IIRC.

Use SmartDashboard in your drivetrain subsystem’s periodic() to publish the left and right wheel encoder position to make sure the values are changing the way you expect them to. Putting the X/Y/heading from the Pose2D on Smartdashboard is helpful for debugging as well.

Can you link to your drive subsystem code?

Have now made significant changes and the robot now follows the path but after the turn it stays still going back and forth.



VIDEO

I think you need to call .setRightSideInverted(false); on your DifferentialDrive object and manually invert the motors in your drive subsystem.

This is what our team’s drive subsystem looks like: 2021-Robot-Code/driveSubsystem.java at main · FRC-Sonic-Squirrels/2021-Robot-Code · GitHub