I have been trying to use Pathweaver and have used the wpilib screensteps but instead of my code following the trajectory, my robot is going crazy fast and the robot code then times out and turns red on Driver Station. This is my code:
public void path(String pathName){
Trajectory leftTrajectory = PathfinderFRC.getTrajectory(pathName + ".right" );
Trajectory rightTrajectory = PathfinderFRC.getTrajectory(pathName + ".left");
leftEncoderFollower = new EncoderFollower(leftTrajectory);
rightEncoderFollower = new EncoderFollower(rightTrajectory);
leftEncoderFollower.configureEncoder(leftEncoder.get(), 128, 0.152);
rightEncoderFollower.configureEncoder(rightEncoder.get(), 128, 0.152);
leftEncoderFollower.configurePIDVA(p, i, d, v, .00);
rightEncoderFollower.configurePIDVA(p, i, d, v, 0.00);
Notifier notifier = new Notifier(this :: followPath);
notifier.startPeriodic(leftTrajectory.get(0).dt);
}
public void followPath(){
if(leftEncoderFollower.isFinished() || rightEncoderFollower.isFinished()){
notifier.close();
}
else{
double l = leftEncoderFollower.calculate(leftEncoder.get());
double r = rightEncoderFollower.calculate(rightEncoder.get());
double gyroHeading = gyro.getAngle();
double desiredHeading = Pathfinder.r2d(leftEncoderFollower.getHeading());
double angleDifference = Pathfinder.boundHalfDegrees(desiredHeading - gyroHeading);
double turn = 0.8 * (-1./80) * angleDifference;
leftPathSpeed = (r + turn);
rightPathSpeed = (l - turn);
tankDrive(rightPathSpeed , leftPathSpeed);
}
}