# SmoothPathPlanner help!!š

So currently our programming team(me) is working on implementing the Smooth Path Planner into our auto code for next year. I have a good understanding of the basics of what it does,however how do I actually take the velocity profiles and implement them into the driving motors. I would preferably like to just use straight motor speeds, because I do not have much experience with PID and Motion Profiling besides programming a simple arm PID that just uses a P value.

Smooth Path Planner can be found here:

This is currently what I have right now( sorry if it is rough I only spent a little bit of time on it). It is just in a switch function for choosing auto paths.

``````

double autoPathCoordinates]] = { { 0, 0 }, { 1, 3 }, { 4, 3 }, { 5, 2 } };

final FalconLinePlot pathPlot = new FalconLinePlot(autoPathCoordinates);

final FalconPathPlanner autoPath = new FalconPathPlanner(autoPathCoordinates);

double totalTime = 4;
double timeStep = .1;
double robotTrackWidth = 2.3; //could be 2.1

autoPath.calculate(totalTime, timeStep, robotTrackWidth);

double leftVel = autoPath.smoothLeftVelocity[index][1]/10;
//my attempt at trying to convert to motor speeds
double rightVel = autoPath.smoothRightVelocity[index][1]/10;

final FalconLinePlot velocityPlot = new FalconLinePlot(
new double]] { { 0.0, 0.0 } });

velocityPlot.xGridOn();
velocityPlot.yGridOn();
velocityPlot.setYLabel("Y (velocity(ft/s))");
velocityPlot.setXLabel("X (seconds)");

oi.drive.leftSet(leftVel);
oi.drive.rightSet(rightVel);

index++;

``````

Personally, I would not recommend to attempt this without closed-loop control.

If you do attempt to do it without closed loop:

I assume autoPathCoordinates is in units of āftā?

Falcon path planner will output velocities therefor in ft/sec.

To convert this to a motor command in an open loop fashion, you will need to make some assumptions about what motor commands correspond to velocities.

I would start with a linear assumption: 0.0 command is 0 ft/sec, 1.0 command is your robotās maximum speed, 0.5 command is half your robotās max speedā¦

Multiply the ft/sec number returned by pathplanner by a constant to get the motor command.

Note this neglects the dynamics of acceleration (as well as many other things). You will likely have incorrect and inconsistent results. A more complete open-loop solution will involve characterizing your drivetrain (see some of Oblargās papers) and doing a more complex conversion to motor command that involves math beyond what I know how to do. This will still not get you perfectly correct though - as soon as you grease up your gearboxes, your auto routines will start to go further.

This is probably a poor example, but hereās our Autonomous code from 2016 that used smooth path planner. Note we did use PID control and gyro feedback.

A word of caution too if you reference our code - FalconPathPlanner has someā¦ interestingā¦ behaviors in certain scenarios. Our paths were very simple. We did forkit at one point to fix up some of that, and had some stuff in 2017 to attempt to coerce it to work for mechanum.

Should I just put the calculation and the talon assignments in a loop or the whole function, such as

``````

while(index <= autoPath.length){

double leftVel = autoPath.smoothLeftVelocity[index][1] / maxVel;
double rightVel = autoPath.smoothRightVelocity[index][1] / maxVel;

System.out.println("left length: " + autoPath.smoothLeftVelocity.length);
System.out.println("right length: " + autoPath.smoothRightVelocity.length);

oi.drive.leftSet(leftVel);
oi.drive.rightSet(rightVel);

index++;
}

``````

or should I have the whole code that is managing the smooth path planner in a while loop or a for loop.

The fundamental process is:

1. Define waypoints
2. Calculate trajectories using a pathplanner library
3. At every time step, assign the velocity associated with that time step in the trajectory to the wheels

We are now attempting to use velocity PID

and such to be more accurate with our auto paths, however our team is confused as to how to convert the units outputted by the SmoothPathPlanner to input them into the

leftFront.set(ControlMode.Velocity, leftSpeed);
rightFront.set(ControlMode.Velocity, rightSpeed);

If the units outputted are feet per second, then how do we convert those so that the code above will work.

It should just be unit conversion like they do in most science classes.

Perhaps the crucial fact to remember is that there is `2 * PI * <wheel radius>` distance of linear robot travel per wheel revolution.

1 Like