Motion Profiling

[Jared]

Originally Posted by Ether

[i]

Step1:
Determine how far you want the robot to move, how fast you want to allow it to accelerate at startup (and decelerate as it approaches the destination), and the max speed limit during that motion.

Step2:
Run the calculator to get your distance vs time equations.

Step3:
Each pass through TeleOp, chose the appropriate x(t) equation based on elapsed time "t" since start, and use that equation to compute a new position setpoint.

Step4:
Use the setpoint from Step3 as the position command to a closed-loop controller controlling whatever it is you want to control (e.g. wheels of a drivetrain, position of an elevator, etc)

We've done something very similar to what you have described here for motion profiling, but we've used a different method to follow the profile which works very well.

We started with the same profiling setup that you use, but we found that the wheels on the robot would slip during faster acceleration, so we switched to a jerk limited control - using trapezoids for acceleration instead of velocity. It just adds another degree of smoothness which helped for fast moves.

We are using something that was described to me by a member of team 254 at St. Louis last year.

We start by coming up with a big table of position, velocity, and acceleration for each .05 seconds that meet the desired position, max velocity, max acceleration, and max jerk goals. We have a thread that runs every .05 seconds and reads the next position, acceleration, and velocity values from the list. It takes the roboRIO about 100ms to generate all possible profiles that we would want to run and store them in memory.

The output for our motors is

output = k_a * acc + k_v * vel + k_p * (desired-actual)

This uses proportional feedback control like P in PID to correct for any misalignment, but most of the output comes from the velocity and acceleration feed forward. It's accurate to within 2" or so with no feedback at all using only well tuned feed forward constants.

Our method to calculate the motion profile isn't super efficient, super fast, and uses lots of memory, but it was written in a really short amount of time. We start by generating the acceleration ramp up part until we reach maximum acceleration. Next, we continue at maximum acceleration until we reach half of our maximum velocity. At that point, we mirror the acceleration curve so that the acceleration ramps down as we reach our maximum speed. The last step is to continue at maximum velocity until we reach half our distance setpoint, and then we mirror everything.

You can check out the code for making profiles here
https://github.com/dicarlo236/2015-R...Generator.java

Our follower is here
https://github.com/dicarlo236/2015-R...eFollower.java

You can see it running in this video
https://www.youtube.com/watch?v=sGQk0-u0HMI