Hello!
I’ve been trying to implement trajectories following this guide on WPILIB’s website
https://docs.wpilib.org/en/stable/docs/software/pathplanning/trajectory-tutorial/index.html
I’ve followed every step, but when I enable autonomous it blasts in reverse at max speed.
Is there any way I can fix this? This is our first year trying to use encoders and gyros on our robot.
Here’s the RamseteCommand itself.
Autos.Java
public static Command ramseteCommand(DriveTrain drive) {
var autoVoltageConstraint =
new DifferentialDriveVoltageConstraint(
new SimpleMotorFeedforward(
voltConstants.ksVolts,
voltConstants.kvVolts,
voltConstants.kaVolts),
voltConstants.kDriveKinematics,
10);
// Create config for trajectory
TrajectoryConfig config =
new TrajectoryConfig(
voltConstants.kMaxSpeedMetersPerSecond,
voltConstants.kMaxAccelerationMetersPerSecondSquared)
// Add kinematics to ensure max speed is actually obeyed
.setKinematics(voltConstants.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, 0), new Translation2d(2, 0)),
// End 3 meters straight ahead of where we started, facing forward
new Pose2d(3, 0, new Rotation2d(0)),
// Pass config
config);
drive.returnField2d().getObject("traj").setTrajectory(exampleTrajectory);
RamseteCommand ramseteCommand =
new RamseteCommand(
exampleTrajectory,
drive::getPose,
new RamseteController(voltConstants.kRamseteB, voltConstants.kRamseteZeta),
new SimpleMotorFeedforward(
voltConstants.ksVolts,
voltConstants.kvVolts,
voltConstants.kaVolts),
voltConstants.kDriveKinematics,
drive::getSpeeds,
new PIDController(voltConstants.kpDrive, 0, 0),
new PIDController(voltConstants.kpDrive, 0, 0),
// RamseteCommand passes volts to the callback
drive::tankDriveVolts,
drive);
// Reset odometry to the starting pose of the trajectory.
drive.resetOdometry(exampleTrajectory.getInitialPose());
// Run path following command, then stop at the end.
return ramseteCommand.andThen(() -> drive.tankDriveVolts(0, 0));
}
}
and here’s our DriveTrain subsystem
DriveTrain.java
public class DriveTrain extends SubsystemBase {
//declare our motor controllers in order to manipulate them.
private final WPI_VictorSPX topLeftMotor = new WPI_VictorSPX(Constants.driveMotors.m_topLeftMotor);
private final WPI_VictorSPX bottomLeftMotor = new WPI_VictorSPX(Constants.driveMotors.m_bottomLeftMotor);
private final WPI_VictorSPX topRightMotor = new WPI_VictorSPX(Constants.driveMotors.m_topRightMotor);
private final WPI_VictorSPX bottomRightMotor = new WPI_VictorSPX(Constants.driveMotors.m_bottomRightMotor);
private final MotorControllerGroup leftmotors = new MotorControllerGroup(topLeftMotor, bottomLeftMotor);
private final MotorControllerGroup rightmotors = new MotorControllerGroup(topRightMotor, bottomRightMotor);
//sets the motors to drive together in tandem
private final DifferentialDrive drive = new DifferentialDrive(leftmotors, rightmotors);
//Declaring encoders to figure out distance traveled
private final Encoder leftEncoder = new Encoder(2, 3, false, EncodingType.k1X);
private final Encoder rightEncoder = new Encoder(0, 1, false, EncodingType.k1X);
//Declaring a gyro to allow us to know which direction the robot is in.
private final AHRS gyro = new AHRS(SerialPort.Port.kMXP);
private final Field2d m_field = new Field2d();
//allows us to have a consistent acceleration instead of jumping straight to speed.
//Wprivate final SlewRateLimiter limiter = new SlewRateLimiter(1.2, 0.2);
private ShuffleboardTab main = Shuffleboard.getTab("Main Data");
private final DifferentialDriveOdometry odometry;
/** Creates a new DriveTrain. */
public DriveTrain() {
topRightMotor.setInverted(true);
leftmotors.setInverted(true);
gyro.reset();
//leftEncoder.setDistancePerPulse(Constants.driveMotors.distancePerPulse);
//rightEncoder.setDistancePerPulse(Constants.driveMotors.distancePerPulse);
resetEncoders();
odometry = new DifferentialDriveOdometry(gyro.getRotation2d(), leftEncoder.getDistance(), leftEncoder.getDistance());
main.add("Left Encoder", leftEncoder);
main.add("Right Encoder", rightEncoder);
main.add(gyro);
main.add(drive);
main.add("Left Motors",leftmotors);
main.add("Right Motors",rightmotors);
main.add(m_field);
}
/** Allows the robot to actually drive
* @param speed How fast the robot should move forward
* @param rotation How fast the robot should turn
*/
public void drive(double speed, double rotation){
drive.arcadeDrive(speed, rotation);
}
public void getDirection(){
gyro.getYaw();
}
public void resetEncoders(){
leftEncoder.reset();
rightEncoder.reset();
}
public DifferentialDriveWheelSpeeds getSpeeds(){
return new DifferentialDriveWheelSpeeds(leftEncoder.getRate(), rightEncoder.getRate());
}
public Pose2d getPose(){
return odometry.getPoseMeters();
}
public void tankDriveVolts(double leftVolts, double rightVolts){
leftmotors.setVoltage(leftVolts);
rightmotors.setVoltage(rightVolts);
drive.feed();
}
public void resetOdometry(Pose2d pose) {
resetEncoders();
odometry.resetPosition(
gyro.getRotation2d(), leftEncoder.getDistance(), rightEncoder.getDistance(), pose);
}
public Field2d returnField2d(){
return m_field;
}
@Override
public void periodic() {
// This method will be called once per scheduler run
odometry.update(gyro.getRotation2d(), leftEncoder.getDistance(), rightEncoder.getDistance());
m_field.setRobotPose(odometry.getPoseMeters());
}
}