org.usfirst.frc.team1736.lib.FalconPathPlanner

Class FalconPathPlanner

java.lang.Object

org.usfirst.frc.team1736.lib.FalconPathPlanner.FalconPathPlanner

public class FalconPathPlanner
extends java.lang.Object

This Class provides many useful algorithms for Robot Path Planning. It uses optimization techniques and knowledge of Robot Motion in order to calculate smooth path trajectories, if given only discrete waypoints. The Benefit of these optimization algorithms are very efficient path planning that can be used to Navigate in Real-time. This Class uses a method of Gradient Decent, and other optimization techniques to produce smooth Velocity profiles for both left and right wheels of a differential drive robot. This Class does not attempt to calculate quintic or cubic splines for best fitting a curve. It is for this reason, the algorithm can be ran on embedded devices with very quick computation times. The output of this function are independent velocity profiles for the left and right wheels of a differential drive chassis. The velocity profiles start and end with 0 velocity and maintain smooth transitions throughout the path. This algorithm is a port from a similar algorithm running on a Robot used for my PhD thesis. I have not fully optimized these functions, so there is room for some improvement. Initial tests on the 2015 FRC NI RoboRio, the complete algorithm finishes in under 15ms using the Java System Timer for paths with less than 50 nodes. FRC 1736 Casserole has made some modifications for documentation and exposing more calibration parameters. Hence the version bump.

Field Summary

Fields

Modifier and Type	Field and Description
double[][]	heading Array of expected heading readings (in degrees).
double	numFinalPoints Total number of setpoints generated as part of the path planning process
double[][]	smoothLeftVelocity Array of desired left wheel velocities.
double[][]	smoothRightVelocity Array of desired right wheel velocities.

Constructor Summary

Constructors

Constructor and Description
FalconPathPlanner(double[][] path) Constructor, takes a Path of Way Points defined as a double array of column vectors representing the global cartesian points of the path in {x,y} coordinates.

Method Summary

Modifier and Type	Method and Description
void	calculate(double totalTime, double timeStep, double robotTrackWidth) This code will calculate a smooth path based on the program parameters.
static void	main(java.lang.String[] args)
void	poofExample()
static void	print(double[] path)
static void	print(double[][] path) Prints Cartesian Coordinates to the System Output as Column Vectors in the Form X Y
void	setPathAlpha(double alpha) Advanced path adjustment.
void	setPathBeta(double beta) Advanced path adjustment.
void	setPathTolerance(double tolerance) Advanced path adjustment.
void	setVelocityAlpha(double alpha) Advanced path adjustment.
void	setVelocityBeta(double beta) Advanced path adjustment.
double[][]	smoother(double[][] path, double weight_data, double weight_smooth, double tolerance) Optimization algorithm, which optimizes the data points in path to create a smooth trajectory.

Methods inherited from class java.lang.Object

clone, equals, finalize, getClass, hashCode, notify, notifyAll, toString, wait, wait, wait

Field Detail

smoothRightVelocity

public double[][] smoothRightVelocity

Array of desired right wheel velocities. Use these to assign setpoints to the PID which controls the speed of the right wheels

smoothLeftVelocity

public double[][] smoothLeftVelocity

Array of desired left wheel velocities. Use these to assign setpoints to the PID which controls the speed of the left wheels

heading

public double[][] heading

Array of expected heading readings (in degrees). Use these to assign setpoints to the controller which adjusts wheel speed based on gyro readings vs. heading error.

numFinalPoints

public double numFinalPoints

Total number of setpoints generated as part of the path planning process

Constructor Detail

FalconPathPlanner

public FalconPathPlanner(double[][] path)

Constructor, takes a Path of Way Points defined as a double array of column vectors representing the global cartesian points of the path in {x,y} coordinates. The waypoint are traveled from one point to the next in sequence. For example: here is a properly formated waypoint array double[][] waypointPath = new double[][]{ {1, 1}, {5, 1}, {9, 12}, {12, 9}, {15,6}, {15, 4} }; This path goes from {1,1} -> {5,1} -> {9,12} -> {12, 9} -> {15,6} -> {15,4} The units of these coordinates are position units assumed by the user (i.e inch, foot, meters)

Parameters:

path - array of waypoints to plot a path for.

Method Detail

print

public static void print(double[] path)

print

public static void print(double[][] path)

Prints Cartesian Coordinates to the System Output as Column Vectors in the Form X Y

Parameters:

path -

smoother

public double[][] smoother(double[][] path,
                           double weight_data,
                           double weight_smooth,
                           double tolerance)

Optimization algorithm, which optimizes the data points in path to create a smooth trajectory. This optimization uses gradient descent. While unlikely, it is possible for this algorithm to never converge. If this happens, try increasing the tolerance level. BigO: N^x, where X is the number of of times the while loop iterates before tolerance is met.

Parameters:

path -

weight_data -

weight_smooth -

tolerance -

Returns:

setPathAlpha

public void setPathAlpha(double alpha)

Advanced path adjustment. Experimiental, may cause path non-convergence

Path Beta & Alpha determine the allowed divergence from sharp corners in the waypoint set. Lower alpha allows the robot to diverge more from the waypoints given, but causes direction transistions to be more gradual. Tested reasonable range seems to be around 0.1 to 1.0 or so.

Parameters:

alpha -

setPathBeta

public void setPathBeta(double beta)

Advanced path adjustment. Experimiental, may cause path non-convergence.

Path Beta & Alpha determine the allowed divergence from sharp corners in the waypoint set. Higher beta allows the robot to diverge more from the waypoints given, but causes direction transistions to be more gradual. Tested reasonable range seems to be around 0.01 to 0.6 or so.

Parameters:

alpha -

setPathTolerance

public void setPathTolerance(double tolerance)

Advanced path adjustment. Experimiental, may cause path non-convergence Totally untested by casserole. Probably does something useful.

Parameters:

alpha -

setVelocityAlpha

public void setVelocityAlpha(double alpha)

Advanced path adjustment. Experimiental, may cause path non-convergence

Velocity Beta & Alpha determine the agressivness of acceleration of the drivetrain Lower alpha causes the desired velocity curves to be much smoother and less agressive. Tested reasonable range seemsto be around 0.1 to 0.0001

Parameters:

alpha -

setVelocityBeta

public void setVelocityBeta(double beta)

Advanced path adjustment. Experimiental, may cause path non-convergence

Velocity Beta & Alpha determine the agressivness of acceleration of the drivetrain Higher beta causes the desired velocity curves to be much smoother and less agressive. Tested reasonable range seems to be around 0.1 to 0.8 or so.

Parameters:

alpha -

calculate

public void calculate(double totalTime,
                      double timeStep,
                      double robotTrackWidth)

This code will calculate a smooth path based on the program parameters. If the user doesn't set any parameters, the will use the defaults optimized for most cases. The results will be saved into the corresponding class members. The user can then access .smoothPath, .leftPath, .rightPath, .smoothCenterVelocity, .smoothRightVelocity, .smoothLeftVelocity as needed. After calling this method, the user only needs to pass .smoothRightVelocity[1], .smoothLeftVelocity[1] to the corresponding speed controllers on the Robot, and step through each setPoint.

Parameters:

totalTime - - time the user wishes to complete the path in seconds. (this is the maximum amount of time the robot is allowed to take to traverse the path.)

timeStep - - the frequency at which the robot controller is running on the robot.

robotTrackWidth - - total distance between left and right side wheels of a skid steer chassis. Known as the track width.

main

public static void main(java.lang.String[] args)

poofExample

public void poofExample()