Motion Algorithm Comparisons of Autonomous Routines in the VEX Robotics Competitions


I’ve been working on this paper for about a month now for english, and I felt that my professor just wasn’t enough of an audience for this as much as people on here are. I really hope that other programmers will be able to learn from what I’ve learned and possible even implement some of this stuff.

I would also LOVE any feedback on my research and what all of you have found in your programming experience so please comment below or dm me.

Thanks for reading!


In the VEX robotics Competition (VRC) programmers usually begin coding time-based algorithms for the autonomous portion of the competition. This results in inconsistent, inaccurate chassis movements, resulting in gaining a disadvantage for the remainder of the match.

Only some VEX programmers, sadly, recognize time-based algorithms are ineffective. The “big brained” programmers typically switch their motion logic to either: motion profiling, or a proportional integral derivative loop (PID). Using these advanced algorithms allows programmers (who have a small amount of time) to program a more consistent autonomous routine faster. In this paper, I am going to show research from experts and my own testing to determine the best algorithm for the most accurate control of the robot’s movement.

Now the two simplest ways to program an autonomous (besides time-based) are either using PID or Motion Profiling. If you aren’t already familiar with both of these algorithms, then you should. PID (a position, integral, and derivative) controller is the simplest of the closed loop controllers. A closed loop controller is simply a function with an input and an output with certain parameters that changes what the output will be (chassisControllerPid.cpp).

An example of a closed loop PID controller would be: a V5 motor turning a certain amount of degrees and getting the error (amount of degrees until the target degree) using the integrated encoder, and manipulating variables to control the output motor velocity or voltage. Thus making the motor slow down enough to be at the correct input (Gillard 2-16).

Motion profiling (sometimes called ramping) is somewhat similar to PID where it slows down to meet the desired target; however, it also limits the kinematics for speeding up to meet the desired target velocity (Lewin 1-5). Assuming the kinematics for the robot are correct, the motion profile is accurate.

Now PID and motion profiling isn’t everything in an autonomous routine. They have to be implemented properly. The best way to implement any algorithm in high school VEX robotics is to use three wheel encoder odometry. Odometry, if built and tuned correctly, is an absolute positioning system for skid-steer robots (a chassis where all wheels are parallel) both holonomic and nonholonomic.

Team 5225A, the Pilons, developed and published a paper that shows exactly how to implement odometry with a VEX robot. In this paper they give some suggestions on how to implement using odometry (with a control algorithm like motion profiling or PID) and one of such suggestions is: “[N]o parameters should indicate a value relative to your current position or orientation; they should be field-centric. This means you don’t say turn 25 degrees to the right, you say turn to face 90 degrees [clockwise] from the audience direction,” (5225, 9).

This is something that I believe should be used much more frequently in autonomous routines across the board. If you move or turn relative to the current position, the robot can be in the wrong position, and it will ruin the rest of your routine. Having a way to specify to turn to a direction or move to a certain point. rather than relative to the previous position or angle of the robot. Doing this will help your routine be more consistent, but not necessarily accurate for a given movement.

In addition to odometry making routines more consistent, there is another use of odometry enables the programmer to specify a path and follow it. This is called a path follower. Path followers are robot controllers that use odometry, and a motion algorithm to follow a path. Points are defined relative to the robot’s starting orientation and position, a path generated using a number of interpolation algorithms (Ravankar, 1). The path is then followed by the robot using a path follower.

One such algorithm is called pure pursuit. The algorithm works as, what Coulter says, “a method for calculating the arc necessary to get a robot back onto a path … We tend to think of the vehicle as chasing a point on the path some distance ahead of it - it is pursuing that moving point,” (Coulter, 3). Later in Coulter’s paper, it says that it is often compared to humans driving a car where they look at a point and move based off of that point. Basically, pure pursuit is a path following algorithm that purely follows a point that is specified on the path at a certain distance ahead of the robot’s position.

The first paper on pure pursuit is published by The Robotics Institute at Carnegie Mellon University, and written by Craig Coulter. In this paper, the Coulter says that the pure pursuit algorithm is being used on several professional robots that the author has helped to develop. This means that the pure pursuit algorithm is extremely robust, and has a lot of capability for programmers.

Now, at this point of the paper, I could reference and explain how other path followers such as the ramsete controller (Veness, 1) work; however, according to Elbanhawi’s research, “Pure pursuit outperformed other controllers under sto-chastic actuation condition and discontinuous paths. These kinematic controllers were limited to low speed and small changes in steering as they ignore the dynamics of vehicle, ” (Elbanhawi, 620).

However, this paper is mainly highlighting the PID and motion profiling algorithms. Just realize that, according to Coulter, the biggest disadvantages of pure pursuit are, “A sharp change in curvature can be requested at a high speed, causing the vehicle’s rear end to skid. The vehicle will not close on the path as quickly as desired because of the first order lag in steering,” (Coulter, 15).

However, the skidding has been solved by limiting the maximum speed of the vehicle based on the curvature of the path by a library that Theo Lemay has created from team 7842F, and the lag in steering is caused by the lookahead distance. This problem has been solved by a forum post on chiefdelphi by edf42001 that came up with a way to adjust the lookahead distance based on the distance from the path.


In the tests for this paper, I am going to compare the consistency and accuracy of two different algorithms. One algorithm will be motion profiling and the other algorithm will be using three PID loops together (as implemented in OkapiLib). Two of the algorithms are used for going straight. The first one is distance and the second one is angle. Using both of these algorithms will allow the robot to go in a straight line that is a certain distance away either forward or backward.

The second PID loop is for turning. The other algorithm is a motion profile controller. For this, two different controllers are to be used left, and right, to make turns and go straight. For this controller I am going to basically use the default algorithms from okapi (main.cpp). The implementation for these algorithms are in (main.cpp). I am going to use the ChassisControllerPID (chassisControllerPID.cpp), and two of the AsyncLinearMotionProfileController (asyncLinearMotionProfileController.cpp) class in a SkidSteerModel configuration (main.cpp).

During these tests, the battery will provide consistent voltage to the motors (so that voltage is always greater than 13 volts). For tuning PID, I will use George Gillards commonly used guide by VEX Robotics programmers to learn and tune their PID. I will use the information from the motion profiling pdf from Chuck Lewin on the topic for tuning profiling. Then I will log the kinematics of the robot. Each iteration of tuning will include executing a 24 inch movement or 90 degree turn both forward and back.

When these are both tuned as properly as possible, I will log each algorithm’s movement time, position, velocity, acceleration, jerk, and snap and put it all in a graph (kinematicsLog.cpp). These graphs will help visualize what each algorithm does in it’s process. After tuning each algorithm properly, I will then make and tune a simple three point Tower Takeover autonomous routine with our competition (tray) robot with each of the algorithms.

This will be the test to determine which algorithm is more accurate, and possibly a custom algorithm that is able to control the robot more effectively than either of the algorithms. The test will include five runs of both the motion profile and PID algorithm. If a test fails it will be considered invalid outlier the data from the test will not be used. While performing each test, the robot will be logging the kinematics and position data over time with a delay of 50 milliseconds (main.cpp).

In the process of writing this paper, the motion profile parameters weren’t tuned as well as they could have been implemented. I didn’t tune the motion profile as well as it could have been tuned (PID took ~9.5 seconds, while motion profiling took ~12 seconds for the routine) until it was too late, and a friend (Salmon from the VEX Robotics Unofficial Discord) pointed out to me that the motion profile movement should take about as long as the PID movement. Because of this, we are not going to take the amount of time that each algorithm takes into whether one is better. I might come out with another test where I have tuned the same robot’s motion profile better and modify this paper.

Ex. 1: Motion Profile Graph Beginning (RED: Position, BLUE: Velocity, Green: Acceleration)

Example one and two, of the tests performed, is the start of the autonomous. The robot starts with a cube in the tray, moves forward 12 inches, sets the intake to in full, moving forward 15 inches to grab two cubes, move backwards -5.5 inches, turning ~145 degrees, and sets the intake to out half power for half a second. In the process of the first movement, notice the motion

Ex. 2: PID Graph Start (RED: Position, BLUE: Velocity, Green: Acceleration)

profile’s acceleration. It starts to deviate from one another at the beginning of reaching the maximum velocity. With PID, the inconsistencies start to happen when it starts to slow down. They both need work to be more accurate, but you wouldn’t be able to tell the deviation unless it is graphed.

The same inconsistencies are minimal in both of the algorithms in the next several movements, however the PID algorithm shows an early start of being more inconsistent from run to run. This is because PID is a closed loop control algorithm, while a motion profile is an open loop control algorithm. A closed loop algorithm will adjust for subtle changes in the environment, while an open loop doesn’t adjust for them and you need to send in the extra variables and logic to make sure that the autonomous is correct.

Ex. 3: Motion Profile Graph Ending (RED: Position, BLUE: Velocity, Green: Acceleration)

In graphs three and four, the second part of the autonomous mode occurs. During this movement, the robot: moves forward 17 inches, sets the tilter for the tray to go up, delays 1.9 seconds, sets the intake to out half power, waits for the tilter to settle, sets the tilter down, and moves backward 17 inches. In the movement, there is a large change in the center of mass in the robot (the tray and cubes), so it is expected that there is more variance in the kinematics.

If you look at the first peak in acceleration, (green) in the second graph, one of the lines settled later compared to the other lines. This tends to happen with settlers because each run is different. Despite that some runs take a little bit longer, the algorithm still works, and the change in real life is so slim that you can’t tell.

The biggest thing that I noticed during the movement after placing a stack is: when the PID is run, it fully throttles the robot backwards. This resulted in vibrations in the tray and robot. While this can be fixed with having less vibration in the robot, the motion profile algorithm was just as smooth as usual with the normal parameters. This is a good example of the biggest flaw with PID: a common way to limit how fast the robot goes is to adjust the voltage.
Ex. 4: PID Graph Ending (RED: Position, BLUE: Velocity, Green: Acceleration)

However adjusting the voltage, with a PID loop, will adjust the gains of the loop enough to mess it up. To do this, lower the max output voltage, and adjust your parameters (retune) when making movements that need to accelerate slower. This process is tedious and annoying. It is common that there are two or three fully tuned PID variables to manage for a routine, and tuning one set of PID variables takes a lot of time.

Example five is a comparison of the position data of the robot during the autonomous routine between PID (purple) and motion profiling (orange). While PID has slight variations in following the path that can and will grow to accumulate error, the motion profiling movement is clearly not meant for precision and will accumulate error over time. This lack of precision is the biggest problem with motion profiling.

It just can’t compete with PID on precision, because it is inherently an open loop movement algorithm. In autonomous modes, this lack of precision is often combated by pushing the robot against a wall of the field to straighten the robot directly

Ex. 5: Motion Profile (orange) vs. PID (purple) Position

before performing an action that needs precision. This is a good way to combat this inaccuracy, but motion profiling just clearly can’t quite be as precise as PID.


In a recent Tower Takeover robot explanation on youtube, the programmer said something that has stuck with me, “We do all motion profiles everywhere, so we’ll run the motion profile and then the last like inch will be a PID loop,” (2114X Tower Takeover Robot Explanation). This is brilliant. They combine the best of both motion profiling and PID to make the movement accurate and kinematically consistent, however is there a better way?

The biggest problem with the default PID algorithm is that there is no way to adjust the initial movement kinematics to the robot’s specifications, so all that needs to be changed is: simply run a profile for the acceleration of the robot, and then start a pid loop that will decelerate the robot effectively. This adjusts the initial movement for PID (which isn’t factored into the algorithm). While this hasn’t been tested, if implemented properly, it would be vastly superior to the default PID algorithm, because it fixes the biggest problem with it.

Ex. 6: PID Forward and Back Movement (Blue is Position, while Red is Velocity)

The biggest problem with only using the motion profile algorithm is the fact that it can’t adjust to correct for disturbances (due to it being open loop). This can be solved by what PID does with the integral variable (ex. 6). It has an output for when the robot is at the very least voltage or velocity that it can be while still providing movement.

This can be implemented with the open loop motion profile algorithm by running the movement. After running the motion profile, it has a simple closed proportional loop that outputs the minimum moving velocity (either positive or negative depending on the error) with an exit loop settler. This will enable the robot to get to the desired position (be accurate) without running a PID loop.

In finale, it doesn’t matter which algorithm you use, because motion profiling and PID are arbitrary algorithms that get close to the right kinematics, but they aren’t perfect in all scenarios. Therefore, the implementation in OkapiLib could allow for much more accurate control of the robot. While odometry on top of these algorithms makes it much easier to consistently control, the target needs to be able to be reached accurately, and then check for disturbances with odometry to move based from the real time position feedback.

Works Cited

  • 5225, Team. “Introduction to Position Tracking.” Technical Resources, The Pilons, 7 Oct. 2018,

  • Alouache, Ali, and Qinghe Wu. "Fuzzy Logic PD Controller for Trajectory Tracking of an Autonomous Differential Drive Mobile Robot (i.e. Quanser Qbot)." The Industrial Robot, vol. 45, no. 1, 2018, pp. 23-33. ProQuest,, doi:

  • asyncLinearMotionProfileController.cpp. commit 6f2ad34a06f23b77e91a2cff63939bd7370c7f6b, OkapiLib, 2020. Github,

  • chassisControllerPid.cpp. commit 5cec774037b57f2a0e1f8d70c32117990dc8bca3, OkapiLib, 2020. Github,

  • chassis.cpp. revision 1, acetousk, 2020. Github,

  • Coulter, R Craig. “Implementation of the Pure Pursuit Path Tracking Algorithm.” The Robotics Institute, Carnegie Mellon University, Jan 1992, Pittsburgh, PA. 23 Sep 2019.

  • edf42001. “Implementation of the Adaptive Pure Pursuit Controller.” Chief Delphi, Team 1712, 18 Aug. 2018,

  • Elbanhawi, M., et al. “Receding Horizon Lateral Vehicle Control for Pure Pursuit Path Tracking.” JOURNAL OF VIBRATION AND CONTROL, vol. 24, no. 3, pp. 619–642. EBSCOhost, doi:10.1177/1077546316646906. Accessed 13 Feb. 2020.

  • Gillard, George. “An Introduction to PID Controllers.” George Gillard, 22 July 2017,

  • Girbés, Vicent, et al. “Path Following Hybrid Control for Vehicle Stability Applied to Industrial Forklifts.” Robotics and Autonomous Systems, vol. 62, no. 6, June 2014, pp. 910–922. EBSCOhost, doi:10.1016/j.robot.2014.01.004.

  • kinematicsLogging.cpp. revision 1, acetousk, 2020. Github,

  • Lewin, Chuck. “Mathematics of Motion Control Profiles.” Performance Motion Devices, Semantic Scholar, 2007,

  • main.cpp. commit ae14d4bd982df6aff4e85126ac33334add1c959a, Team 914, 2020. Github,

  • positionLogging.cpp. revision 1, acetousk, 2020. Github,

  • Ravankar, Abhijeet, et al. “Path Smoothing Techniques in Robot Navigation: State-of-the-Art, Current and Future Challenges.” SENSORS, vol. 18, no. 9. EBSCOhost, doi:10.3390/s18093170. Accessed 13 Feb. 2020.

  • Veness, Tyler. “Ramsete Controller¶.” Ramsete Controller - FIRST Robotics Competition Documentation, WPILib Suite, 6 Jan. 2020,

  • “2114X Tower Takeover Robot Explanation.” Youtube, Cobra Kai, 14 Mar. 2020,


I am tempted to post a video of the auton. Is anyone interested?


Yes please


This was very good! It helped me understand what I need to be doing to make my autons more consistent. Thank You!


What can I say other than absolutely brilliant? A+ from me


This was fantastic, well written, and helped me better understand motion profiling. Hats off to you!


Was curious if you also accounted for other sensors in your research? Was it as simple as that adds a new variable to your data If not, why?

Ignore this I’m an idiot, leaving as a monument to my stupidity


Combining linear PID on a chassis with a “motion profile” or constraining to a constant acceleration can be done in two ways (or any more that you can think of). The first one is with a slew rate controller. This just controls the rate of change of output of the PID controller. So basically before you send power to the chassis, you put the PID output into said controller. The actual logic behind it can be found online as well as the math.

You can also use another PID controller and combine the output with the linear PID controller. First, you find the robot’s current acceleration which could be found with an accelerometer, encoders, odometry (using the global position vector), ultrasonic, etc, etc… There are many ways to find the current acceleration of the robot. Using that acceleration, you can compare it to the target acceleration and use PID to add to the linear power. If the robot isn’t moving fast enough, then acceleration power output will add to the linear power, otherwise, it will subtract from the power. Combining PID is as simple as (accelerationPower + linearPower). Think about it, as you approach the correct acceleration, accelerationPower will approach zero, and linearPower will be the only thing powering the robot.

My question is: how do you determine the experimental maximum acceleration of the robot (before wheels start slipping)? And what makes an acceleration the best acceleration other than the amount of time it takes to reach maximum velocity (I guess this depends on the case)?


Yes please

20 characters


I believe that a slew rate controller would essentially dampen the gain on a PID controller. That would work for the initial acceleration well which is an interesting point. However how would you be able to ensure that acceleration, and jerk don’t reach above their maximum? You would have to use them recursively which is interesting.

And using a PID loop recursively inside of itself wouldn’t work as well as a slew rate controller 10 out of 10 times, because you would have to tune variables that are based off of each other (assuming I’m understanding you correctly).

The way that I tune the motion profile was log the kinematics using this and just input like 80% of the maximum / minimum of them. However as stated I didn’t tune them as well as they could have been tuned.


thanks! I’m glad it helped!

No I only used the VEX Encoders with 2.75" wheels rubber banded to the ground.

I want to reiterate this because it is extremely important. Think of what happens in a relative movement. The start of the movement is essentially treating the robot as if it were at zero. This is problematic because the robot has virtually no margin of error, and your PID controllers need to be perfectly tuned to maintain perfect accuracy. Why not instead give your robot room to fail? This is what absolute positioning does.

Here’s an example: let’s say the robot is sent to move forward twice. The robot moves forward and completes the first movement but because PID is not perfect (though it can be really accurate), the robot overshoots the target slightly. Note: you can’t account for the overshoot when you set the target of the movements because of friction. The robot won’t overshoot to the same value every time. If you aren’t controlling velocity, then the momentum will add another variability. Regardless of the different variables at play, you can see that the small errors in the system can accumulate over time. Then the robot executes the next relative movement which essentially zeros out the robot’s position values. Now the robot doesn’t have a measure of how far it needs to move to reach the final point. It just moves the distance that it was told to move from the current point. This will cause for another overshoot to occur. Then let’s say you want the robot to move backwards, not the robot will undershoot the value.

This adds up over time. I noticed the effects of this to be significant enough of a problem that I had to make a change with TT. The loads were so large in this year’s game that the robot could not physically respond to the values that were being sent to it that the robot would either a) oscillate or b) overshoot. I solved this with an absolute positioning system that bases the next movements of the robot on the end position of the previous movements. I also applied this same logic with turning to an angle and following a straight line at a given angle. Even without odometry, you can still have an absolute based position system just by never zeroing out encoder values.


It only works for the initial acceleration. I realize a motion profile maps out the deceleration as well, but you don’t need to really worry about deceleration with PID. Notice how I said:

Keyword “combine”

Though, it does pose an interesting question as to how you would get the robot to decelerate from the max speed at a constant rate. With PID you can always use error to your advantage. You know in order for the robot to stop at an exact point, it has to start slowing down from max speed at a point. You can find the amount of the distance the robot needs in order to come to a full stop either experimentally or mathematically (though mathematically won’t account for real-world forces). The distance to come to a full stop is completely dependent on the deceleration rate that you want.

With that in mind, the point at which the robot starts to slow down never changes right? So with that logic, you can use error to apply subtractive decceleration power to the robot. Kinda like this: if(abs(error) < decelerationDistance) -> (linearPower + decelerationPower). With the same combing PID logic I explained above.

This isn’t really how I would go about doing it. The approach you explain could work, but it certainly doesn’t make sense for the solution I’m describing. Or rather, the solution you wrote your paper on is much more simple and efficent than a recursive PID loop.

The way I did was with a PID data structure that allowed me to create PID instances for any variable I want to control. This allows me to create a general PID method that just calculates output through inputting the PID instance, target value, and processing value as parameters. Then it returns output (the PID instance has all the constants and intergral bound data).

With that I can have a drive function with multiple constraints using the general PID controller I made. Explaining the logic behind it in words is kind of difficult so I’ll just throw up some example code:

void robotMove(){
      linearPower = pidCalculate(linearPID, targetDistance, currentDistance);
      accelerationPower = pidCalculate(accelerationPID, targetAccel, currentAccel);

I hope the function and variable names are self explanatory. The pid method does NOT have a loop inside of it which allows me to calculate various parts of the robot within one loop. This logic applies to any subsystem really.


That is NOT what absolute positioning does. A PID controller needs an exit loop that is tuned to have an acceptable amount of error (and sometimes derivative, for a certain amount time). This is what OkapiLib’s SettledUtil does. An absolute positioning system, simply returns your current position. This is extremely helpful because you can say:

robotMoveToPoint(3_ft, 3_ft);

What the quote is trying to say there is that if your robot messes up, then you can make sure it still works by using logic like


rather than


because sometimes where you want to turn is 89_degrees and that cumulative error over an auton (especially a skills auton) will mess you up.


Do you have code for this? I’m confused how this would work

Doing this you would need to have a different instance of a PID object or a different method for each object because there needs to be data stored (lastError) etc.

1 Like

I guess I worded it wrong, but I wasn’t trying to explain how it works. I was trying to explain why you should use it, even outside of advanced programming schemes being described.

Not exactly… You can still have an absolute angle to base your turns off of. In pros, if you use the new IMU there is no function that zeros the angle. This means if you don’t manually do it, all your turns will be absolute based.

What I was trying to get at is best explained in a diagram, but I’ll try to better explain it in words.
Just to highlight what I’m getting at, I’ll put some example code:


Say the robot overshoots the target distance by some value arbitrary value x, or after the robot exits the loop some external forces cause the robot to shift position in the time between the next movement a distance of x. The robot will have no way of knowing that it is now x units ahead of the target and will proceed to move on the next relative movement which sets all the encoders to 0. Now the robot will execute the next function. The final distance travled by the robot is now 2 feet + x. This accumulates over time of course. It is very important to remember for large loads when moving the robot at full speed will make it very hard for you to control the robot accurately without compromising on time or oscillating (because the robot’s momentum is so great and vex motors only have so much torque, and the wheels only have so much traction).

Using an absolute system, however, allows for you to tell the robot to move 2 feet from the starting point. So now the robot will subtract x from the target of the next movement to end exactly at the point you want to be on.

I hope that clarified it. (Disclaimer*: There are many more cases that need to be accounted for when dealing with absolute position like what happens when the robot turns? What will you base your linear movements off of now that the orientation and encoder values have changed? The easiest solution to this is odometry. Though, you could probably come up with some sort of logic that works soley with encoders and addition (and probably some trig).


Here is the video as promised.


I do, I can share it to you privately so you can understand (though, I’m only gonna share just this, not my whole program). We should talk more about this in pm so not to clutter the thread. Though if anyone else has any questions or thoughts on what we’re talking about they can certainly chime into this thread.

Yeah sorry for not clarifying this. linearPID and accelerationPID both have their own respective instance data that includes everything necessary to calculate the PID. So that includes previous error, tuning constants, integral bounds, etc. The first parameter in pidCalculate is the PID instance you’re passing in. Everywhere that needs a unique part of what’s being controlled in the PID controller is accessed like this example: pid.prevError(where pid is the instance of the PID class being passed in as a parameter.


Thank you, @Potatehoes914M , for posting this! It made my day and was an enjoyable and informative read! The graphs are great visuals supporting your research! :blush: Keep up the good work!