Pathfinding Help

After a few hours of work on motion profiling, we could only get our robot to spin in circles. This robot is 120 pretty slow and has encoders attached to the gearbox along with 4-inch wheels.
We have taken the setup code for java from this Pathfinding library. And here is what we ended up with


//Motion Profiling Setup
	Waypoint] points = new Waypoint] {
		new Waypoint(-4, -1, Pathfinder.d2r(-45)),
		new Waypoint(-2, -2, 0),
		new Waypoint(0, 0, 0)
	};
	
	//Constants
	double _wheelDiameter = 0.1016; //In Meters
	double _ticksPerRevolution = 2905.51921716; //Encoder Ticks Per Revolution
	double _maxVelocity = 1; //Default 2.0
	double _maxAcceleration = 1; //Default 2.0
	double _maxJerk = 30; //Default 60
	double _wheelBaseWidth = 0.775; //Archer .775 (in meters from inside of both wheels)
	
	Trajectory.Config config = new Trajectory.Config(Trajectory.FitMethod.HERMITE_CUBIC, Trajectory.Config.SAMPLES_HIGH, 0.05, _maxVelocity, _maxAcceleration, _maxJerk);
	Trajectory trajectory = Pathfinder.generate(points, config);
	TankModifier modifier = new TankModifier(trajectory).modify(_wheelBaseWidth);
	
	EncoderFollower left = new EncoderFollower(modifier.getLeftTrajectory());
	EncoderFollower right = new EncoderFollower(modifier.getRightTrajectory());
	
	//Drive
	TalonSRX talonL1 = new TalonSRX(11);
	TalonSRX talonL2 = new TalonSRX(12);
	TalonSRX talonR1 = new TalonSRX(13);
	TalonSRX talonR2 = new TalonSRX(14);

	TalonFactory TalonFactory = new TalonFactory( new int]{ 11, 12, 13, 14 } ); //This is just a custom class reset all talons under the specified ids
	
	ADXRS450_Gyro Gyro = new ADXRS450_Gyro();
	
	Joystick controller = new Joystick(0);
	
	public void robotInit() {
		
	}

	public void autonomousInit() {
		talonL2.set(ControlMode.Follower, talonL1.getDeviceID()); //Make the secondary motors followers
		talonR2.set(ControlMode.Follower, talonR1.getDeviceID());
		talonL1.set(ControlMode.PercentOutput, 0); //Make sure the main motors are stopped
		talonR1.set(ControlMode.PercentOutput, 0);
		left.configureEncoder(talonL1.getSelectedSensorPosition(0), (int) Math.round(_ticksPerRevolution), _wheelDiameter); //Configure the encoder
		left.configurePIDVA(1.0, 0.0, 0.0, 1 / _maxVelocity, 0); //Configure the pidva
		right.configureEncoder(talonR1.getSelectedSensorPosition(0), (int) Math.round(_ticksPerRevolution), _wheelDiameter);
		right.configurePIDVA(1.0, 0.0, 0.0, 1 / _maxVelocity, 0);
	}

	public void autonomousPeriodic() {
		if (!left.isFinished()) {
			//Calculate using the new encoder values
			double l = left.calculate(-talonL1.getSelectedSensorPosition(0));
			double r = right.calculate(talonR1.getSelectedSensorPosition(0));
	
			//Calculate turning
			double gyro_heading = Gyro.getAngle();
			double desired_heading = Pathfinder.r2d(left.getHeading());
			double angleDifference = Pathfinder.boundHalfDegrees(desired_heading - gyro_heading);
			double turn = 0.8 * (-1.0/80.0) * angleDifference;
			
			//System.out.println("Left: " +  (l + turn) + ", Right: " + (r - turn) + " Angle: " + Gyro.getAngle() + "");
			
			talonL1.set(ControlMode.PercentOutput, l + turn);
			talonR1.set(ControlMode.PercentOutput, r - turn);
		} else {
			//Make sure the robot is stopped at the end
			talonL1.set(ControlMode.PercentOutput, 0);
			talonR1.set(ControlMode.PercentOutput, 0);
		}
	}

There’s a lot of things which could cause that symptom, not all code related. It’s good you’ve observed the symptom. The next step in the process is to find root cause of the symptom, so you can fix it correctly.

I’d start by taking the gyro out to see if there’s an issue with how the gyro is integrated:
–Set the waypoints so the robot should just drive forward 10 feet
–Set turn to be always = 0.
–Run the routine. Do the wheels go in the correct direction?

If the above test shows proper function, you can conclude that integrating a gyro introduces the symptom. There are many possible causes of this:
–Is the gyro actually reading any number other than zero?
–Do the sign conventions of pathfinder’s desired heading and the gyro reading agree (aka if you run a path which curves to the right do both the gyro reading and pathfinder’s heading increase? both decrease? or do they do opposite things?)
–How big does the value of “turn” get with the gyro re-integrated? If the robot is off by 5 degrees to the left, what value do you expect turn to have? What value does it actually have? Are both of these numbers reasonable? How about 45 degrees to the right?

If the above test still shows issue, then you can conclude that the issue is unrelated to the gyro. There are some additoinal possibilities:
–Is one side of the drivetrain inverted properly? In tank drive, to go forward, one motor set will move forward, while one moves backward.
–Are both sets of motors being commanded properly? You can identify the recieved command by the colors of the lights on the motor controller. Check for electrical issues by ensuring that the colors on the motor controller correspond to the values you are providing in software.
–What software values are being sent to the motor controller (I think you had this in a commented System.out.println()). In teleop, how would you have to move your joysticks to get those same values? What do you expect your robot to do when the joysticks are in that position?
–What does the return value of left.calculate() and right.calculate() represent? What assumptions does Pathfinder make when returning you that value? What does it expect you to do with it?