Jaci's Pathfinder v1 File Issue

[BlueberryPi]

I was setting up a basic pathfinding program in a command-based structure, using the tutorial documentation. From my understanding, the PathfinderFRC.getTrajectory method is supposed to throw the IOException it generates; however, I’ve been getting an error saying that the exception isn’t thrown. Attempting to try/catch it myself breaks other parts of the program; any advice is appreciated. I’m unsure if altering the tutorial to fit a command structure is the cause.

[Amicus1]

Adapting to commands shouldn’t be a problem but I can’t tell without seeing your code. Can you be more specific on Language and what exactly you’re doing?
Also if this is for 2020 path following will be part of WPILIB.

[BlueberryPi]

1. Sorry- forgot some details. Language is Java, code (incomplete) is as follows:
   package frc.robot.commands;

import java.io.File;

import edu.wpi.first.wpilibj.DoubleSolenoid;
import edu.wpi.first.wpilibj.command.Command;
import frc.robot.Robot;
import jaci.pathfinder.Pathfinder;
import jaci.pathfinder.PathfinderFRC;
import jaci.pathfinder.Trajectory;
//import jaci.pathfinder.Waypoint;
import jaci.pathfinder.followers.EncoderFollower;
import jaci.pathfinder.modifiers.TankModifier;

public class SquiggleTurn extends Command {
public SquiggleTurn() {
// Use requires() here to declare subsystem dependencies
// eg. requires(chassis);
requires(Robot.driveSubsystem);

}

EncoderFollower leftEnc;
EncoderFollower rightEnc;
double maxSpeed;
double maxAccel;
double maxJerk;
double wheelbaseWidth = .587375; // TyRapXIX; meters
double wheelDiameter = .1524; // TyRapXIX; meters
double p;
double i;
double d;
double v;
double a;

File leftPath;
File rightPath;

// Called just before this Command runs the first time
@Override
protected void initialize() {

// Set maximum speed, acceleration, and jerk for robot
maxSpeed = 3.0617;
maxAccel = .1;
maxJerk = .1;

leftPath = new File("/home/lvuser/deploy/paths/Straightforward.left.csv");
rightPath = new File("/home/lvuser/deploy/paths/Straightforward.right.csv");

Trajectory leftTraj = Pathfinder.readFromCSV(leftPath); // please work
Trajectory rightTraj = Pathfinder.readFromCSV(rightPath); // please

// Creates modifier object and generates left and right trajectories using the
// original trajectory as the center
TankModifier leftModifier = new TankModifier(leftTraj).modify(wheelbaseWidth);
TankModifier rightModifier = new TankModifier(rightTraj).modify(wheelbaseWidth);

leftEnc = new EncoderFollower(leftModifier.getLeftTrajectory());
rightEnc = new EncoderFollower(rightModifier.getRightTrajectory());

// Encoder Position is the current, cumulative position of your encoder.
// 1000 is the amount of encoder ticks per full revolution -> check this
// Wheel diameter is the diameter of your wheels (or pulley for a track system)
// in meters

leftEnc.configureEncoder(Robot.driveSubsystem.getLeftEncoderPos(), 1000, wheelDiameter);
rightEnc.configureEncoder(Robot.driveSubsystem.getRightEncoderPos(), 1000, wheelDiameter);

// The first argument is the proportional gain. Usually this will be quite high
p = 1;
// The second argument is the integral gain. This is unused for motion
// profiling.
i = 0;
// The third argument is the derivative gain. Tweak this if you are unhappy with
// the tracking of the trajectory
d = 0;
// The fourth argument is the velocity ratio. This is 1 over the maximum
// velocity you provided in the
// trajectory configuration (it translates m/s to a -1 to 1 scale that your
// motors can read)
v = 1 / maxSpeed;
// The fifth argument is your acceleration gain. Tweak this if you want to get
// to a higher or lower speed quicker
a = 0;

leftEnc.configurePIDVA(p, i, d, v, a);
rightEnc.configurePIDVA(p, i, d, v, a);

}

// Called repeatedly when this Command is scheduled to run
@Override
protected void execute() {
double l = leftEnc.calculate(Robot.driveSubsystem.getLeftEncoderPos());
double r = rightEnc.calculate(Robot.driveSubsystem.getRightEncoderPos());

double gyro_heading = 0; // ... your gyro code here ... // Assuming the gyro
// is giving a value in degrees
double desired_heading = Pathfinder.r2d(leftEnc.getHeading()); // Should also
// be in degrees

// This allows the angle difference to respect 'wrapping', where 360 and 0
// are the same value
double angleDifference = Pathfinder.boundHalfDegrees(desired_heading - gyro_heading);
angleDifference = angleDifference % 360.0;
if (Math.abs(angleDifference) > 180.0) {
  angleDifference = (angleDifference > 0) ? angleDifference - 360 : angleDifference + 360;
  // code had this and the last one as 'angleDiff'
}

double turn = 0.8 * (-1.0 / 80.0) * angleDifference;

Robot.driveSubsystem.setTank(l + turn, r - turn);

}

// Make this return true when this Command no longer needs to run execute()
@Override
protected boolean isFinished() {
return false;
}

// Called once after isFinished returns true
@Override
protected void end() {
}

// Called when another command which requires one or more of the same
// subsystems is scheduled to run
@Override
protected void interrupted() {
}
}

2. Could you link to or describe the 2020 pathfinding integration into WPILib? I haven’t heard anything about it, sounds useful.

[Amicus1]

https://docs.wpilib.org/en/latest/docs/software/wpilib-tools/path-planning/index.html