Trajectory following code not working, only setting max speed in reverse

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());
  }
}

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