FRC Pathfinder Reverse Code

Has anyone got a Github with a working Pathfinder implementation? I am currently working on getting motion profiling to work. I have read some articles on how to implement Pathfinder but I’m not sure how I would code reverse.

In addition, if I were to say use something other than encoders as backup for the positioning, where would I override that code?

Funny you should ask, I’ve compiled a huge list of robot code.

https://firstwiki.github.io/wiki/robot-code-directory


// Pathfinder Drive Forward
left = new EncoderFollower(leftTrajectory);
right = new EncoderFollower(rightTrajectory);

left.configureEncoder(Robot.driveTrain.getLeftEncoderValue(), 1440, 0.1667); // Our encoders are 360 counts/rev, but reading it through the Talon SRX multiplies it by 4
right.configureEncoder(Robot.driveTrain.getRightEncoderValue(), 1440, 0.1667); // Wheelbase is 0.1667 yaards
 
left.configurePIDVA(kP, 0, kD, 1 / max_vel, 0);
right.configurePIDVA(kP, 0, kD, 1 / max_vel, 0);

double l = left.calculate(Robot.driveTrain.getLeftEncoderValue());
double r = right.calculate(Robot.driveTrain.getRightEncoderValue());
			
angleDifference = Pathfinder.boundHalfDegrees(Pathfinder.r2d(left.getHeading()) + Robot.driveTrain.spartanGyro.getAngle());
turn = 2 * (-1.0 / 80.0) * angleDifference; // We used 2 as it seemed to work better after trial and error

									
Robot.driveTrain.setDirectDriveOutput(l + turn, r - turn); // Left Side Output, Right Side Output


// Pathfinder Inverted
left = new EncoderFollower(leftTrajectory);
right = new EncoderFollower(rightTrajectory);

left.configureEncoder(Robot.driveTrain.getLeftEncoderValue(), 1440, 0.1667); // Our encoders are 360 counts/rev, but reading it through the Talon SRX multiplies it by 4
right.configureEncoder(Robot.driveTrain.getRightEncoderValue(), 1440, 0.1667); // Wheelbase is 0.1667 yards

left.configurePIDVA(kP, 0, kD, 1 / max_vel, 0);
right.configurePIDVA(kP, 0, kD, 1 / max_vel, 0);

**double l = left.calculate(-Robot.driveTrain.getLeftEncoderValue());** // Negate encoder count so robot thinks its going forward when going backwards
**double r = right.calculate(-Robot.driveTrain.getRightEncoderValue());**

angleDifference = Pathfinder.boundHalfDegrees(Pathfinder.r2d(left.getHeading()) + Robot.driveTrain.spartanGyro.getAngle());
turn = 2 * (-1.0 / 80.0) * angleDifference; // We used 2 as it seemed to work better after trial and error

									
**Robot.driveTrain.setDirectDriveOutput(-(r - turn), -(l + turn));** // Left Side Output, Right Side Output. We use the right output for the left side and the left side output for the right side, then negate everything.

This makes it so that any path that goes ‘forward’ will go ‘backwards’, meaning you would generate the paths with pathfinder just as if you were driving the robot forward. The only exception is that you would need to negate all the angle headings if you’re turning (e.g. going forward +3 yards and turning right (+90 degrees in pathfinder) would still be go forward +3 yards, but turn ‘right’ (-90 degrees, since the robot is going backwards).

Note that all of this is subject to change once Pathfinder is updated in a month or so.

We created a reverse Distance Follower from the one Jaci provided on GitHub. It runs from the last (forward generated profile) segment to the first. Acceleration values need to be negated and position values are subtracted from the final position value so that position numbers increase as the trajectory runs backwards.

So reverse error calculation is (lastSegmentPosition - )seg.position - distance_covered.

When using this reverse follower, negate the robot position values and the speed output to the drives.

The profile can then be run in forward and reverse.