Pathfinder

Technical Programming
1

How do I use pathweaver and pathfinder for command based java code? I have generated this code but all it does is go crazy fast and eventually crashes my robot code. Also it doesnt follow the path. Can someone please help?

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

Two things to do.

  1. If you have a typical gyro, then it gives a + reading for a clockwise rotation where Pathfinder expects this to be a negative gyro direction.
    You can check that easily. If it does then
    double gyroHeading = gyro.getAngle();
    should be double gyroHeading = -gyro.getAngle();

  2. Path Weaver in addition to crossing left and right inverted the heading.(It is in the Screensteps notification)
    So you should have in total
    double gyroHeading = -gyro.getAngle();
    double angleDifference = - Pathfinder.boundHalfDegrees(desiredHeading - gyroHeading);

3

There’s important things that are missing, like what the path is, and what that PIDVA constants are. I posted a methodical process in this thread that would probably help. Pathfinder Problems (C++)

4

The code below makes the robot go crazy fast and doesnt follow my path drawn in Pathweaver. I was wondering if it is the code or if it’s an issue with my velocity and time step set in pathweaver?

import java.io.File;

import edu.wpi.first.wpilibj.Notifier;
import edu.wpi.first.wpilibj.command.Command;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import frc.robot.Robot;
import frc.robot.RobotMap;
import jaci.pathfinder.Pathfinder;
import jaci.pathfinder.Trajectory;
import jaci.pathfinder.followers.EncoderFollower;

public class FollowPath extends Command {
EncoderFollower leftFollower;
EncoderFollower rightFollower;

File leftCSV;
File rightCSV;

Trajectory rightTrajectory;
Trajectory leftTrajectory;

Notifier notifier;

double dt;

double p = 0.8;
double i = 0.;
double d = 0;
double v = 1 / 12;
double a = 0;
public FollowPath(String pathName) {
requires(Robot.chassisSubsystem);

leftCSV = new File("/home/lvuser/deploy/" + pathName + “.left”);
rightCSV = new File("/home/lvuser/deploy/" + pathName + “.right”);

leftTrajectory = Pathfinder.readFromCSV(leftCSV);
rightTrajectory = Pathfinder.readFromCSV(rightCSV);

notifier = new Notifier(new RunProfile());
dt = leftTrajectory.get(0).dt;

System.out.println(“CSV has been locked and loaded”);
}

// Called just before this Command runs the first time
@Override
protected void initialize() {
leftFollower = new EncoderFollower(leftTrajectory);
rightFollower = new EncoderFollower(rightTrajectory);

leftFollower.reset();
rightFollower.reset();

leftFollower.configureEncoder((int)Robot.chassisSubsystem.getEncoderLeft(), 128, 6 / 12);
rightFollower.configureEncoder((int)Robot.chassisSubsystem.getEncoderRight(), 128, 6 / 12);

leftFollower.configurePIDVA( p,  i, d, v, a);
rightFollower.configurePIDVA(p , i, d , v , a);
notifier.startPeriodic(dt);

System.out.println("Initialized");

}

// Called repeatedly when this Command is scheduled to run
@Override
protected void execute() {
}

// Make this return true when this Command no longer needs to run execute()
@Override
protected boolean isFinished() {
System.out.println(“Finished”);
return leftFollower.isFinished() && rightFollower.isFinished();

}

// Called once after isFinished returns true
@Override
protected void end() {
notifier.stop();
Robot.chassisSubsystem.stop();
}

// Called when another command which requires one or more of the same
// subsystems is scheduled to run
@Override
protected void interrupted() {
end();
}
class RunProfile implements java.lang.Runnable{
int segmentNumber = 0;

@Override
public void run(){


double leftOutput = leftFollower.calculate((int)Robot.chassisSubsystem.getEncoderLeft());
double rightOutput = rightFollower.calculate((int) Robot.chassisSubsystem.getEncoderRight());

double gyroHeading = 0;

double desiredHeading = Pathfinder.d2r(leftFollower.getHeading());

double angleDifference = gyroHeading + desiredHeading;

double turn = 0.08 *  (-1. / 80.) * angleDifference;

Robot.chassisSubsystem.tankDrive(rightOutput - turn, leftOutput - turn);

segmentNumber++;

}
}
}

5

This line is most likely causing some trouble. It should be a subtraction and in the opposite order.

Change

double angleDifference = gyroHeading + desiredHeading;

to

double angleDifference = Pathfinder.boundHalfDegrees(desired_heading - gyroHeading);

Check the docs for any other issues;

…because this line is likely wrong in several ways as well, but I guess I can’t speak to how your tankDrive function actually works. What stands out to me is that turn should be added to leftOutput and that left and right are swapped.
Robot.chassisSubsystem.tankDrive(rightOutput - turn, leftOutput - turn);

Finally, read the notice at the top of the doc page I linked. That will make a BIG difference with PathWeaver. We discovered that this weekend as well.

6

I merged two related threads from the same person together.

7

I tried your process and the robot is still not going the distance I want it to based off of path points and speed.

8

I understand your pain. This weekend my students and I took our path-following code down to the basics and built it up step-by-step confirming that all of our assumptions were correct, one thing at time until we could identify where our own code was going wrong. Our students had picked up a bad habit of simply throwing in a negation wherever something was inverted and the code was so full of -(this) and -(that) that I couldn’t keep track which motors/encoders were the ones that actually were driving in which direction.

Start with your motors, do they go forward when you apply positive output with the joystick? Do you need to invert one?
Next your encoders. Are they reporting? Are they increasing when driving in the forward direction? Do you need to invert one?
Do you have the correct pulse-per-rotation value? Can you confirm it by simply pushing the robot ahead a few feet?
Check your gyro heading when you turn counter-clock-wise, does it increase? Do you need to invert that?

The PathFinder code needs to be changed according to the docs, swapping left from right and inverting the desired heading, otherwise it can be used exactly as given if all these other things are correct. If you have inverted motors, encoders, or a gyro it will make it more confusing to get right.

Take out the gyro turning code from the calculation. It’s only needed to correct for drift, so your robot should still mostly follow the path without it. Use a simple short straight path as a test.
If you pass that, then all your problems are with how you’re using the gyro.

This is the procedure we followed this weekend and it got us from a spinny-boi-bot to one that follows paths within reason.

Note, it’s been noticed that the algorithm does not really follow-through on the final heading value and I think it may be necessary to repeat the final segment a few times to get your robot to hit the final mark. Another modification would be to ensure that your output values are above the minimum necessary to move your robot, especially when turning. However, you can adjust for these issues once you’ve confirmed that the basic algorithm works.