Tustin Code

Lesson 32 - 1D Motion Profiles


This lesson referenced https://www.motioncontroltips.com/what-is-a-motion-profile/.

Prerequisites: 

  • Basic understanding of velocity, acceleration, and position (aka kinematics)

  • High School Level Algebra

  • Good Googling Skills and Persistence in case anything gets confusing 



Motion Profiling 

In Unit 6, when talking about the PID Controller, we talked about a limitation of the PID Controller. When the robot is resting and far away from the target, the PID controller will immediately apply a high power to the robot since the error is large. 


We showed a solution to this issue of high acceleration by introducing the slew controller. Recall that average acceleration is velocity over time. The slew controller is able to limit the acceleration of the PID controller by essentially determining the change in PID power over a small interval of 25 / 50 ms. Then, if the PID power changes by more than a certain max cap, then instead of assigning the robot the new PID power, we simply add the slew amount to the current power. To reference the slew algorithm, please reference a previous chapter. 


Well slew is one way to go about fixing the high-acceleration issue of PID. There are other movement methods called “motion profiles” that can fix the issue of PID. However, they are very different from the slew controller. In the rest of this section, we will talk about 1 dimensional motion profiles. What this means is that the motion profiles we will talk about only control motion in 1 dimension (linear movement). Here is the outline for this section. 


  1. First, we will briefly talk about what motion profiling means. 

  2. Explore the Triangle Motion Profile Shapes 

  3. Explore the Trapezoidal Motion Profile Shapes 

  4. What we would recommend for accurate movement



What are Motion Profiles

Motion Profiles are predetermined velocities for the robot to move at when at a certain time. These velocities are generated based on certain criteria such as (1) time you want for the movement to complete (2) max acceleration (3) max speed (4) distance travelled. 


For example, let’s suppose we want to travel 120 inches 2 seconds (1500 ms) with a max acceleration of 100 inches per second squared and max speed of 70 inches per second. There is indeed a way to generate formulas to determine the velocity the robot should move at during every point in time. Once again, this process of determining what velocity the robot should move at is known as motion profiling. Then, we would simply use the .move_velocity(int velocity) method to have the motors move at that specified velocity (unless you’re feeling extra and want to write your own velocity PID). 



Types of Motion Profiles and Their Velocity vs Time Curves

In general, for linear movement, there are 3 types of “shapes” that the motion profile could look like. The velocity vs. time graphs could look like (1) Triangles (2) Trapezoids (3) “S-Curves.” You may first have trouble deciphering what the graphs represent. They tell you what velocity the robot should move at when at a certain time. The y axis will be velocity and the x axis will be time. The graph indicates that at “x” time the robot will need to move at “y” velocity for that motion profile. If a robot follows one of these motion profiles, the robot should move smoothly. 


We will only discuss the first two since the mathematical maturity required for S-Curves is beyond scope for our average intended audience. 



Triangular Motion Profile (sourced from Motion Control Tips)


This is what a triangular motion profile would look like. As you can see, the robot would first start off at 0 velocity, then it would slowly accelerate. The velocity of the robot over time will be a straight line, as you can see in the graph. With this the acceleration is constant. Moreover, you notice that the graph is symmetrical. Once the robot hits the halfway point, the robot will accelerate with the same constant acceleration but NEGATIVE. 


Here are the equations for the triangle motion profile. 


In other words, as you can see, you will be able to manipulate the previous 3 key equations to determine what velocity the robot should move with for all times. Here is sample code for the triangle motion profile with the parameters being the distance we wish to travel, and the total time we wish for the movement to take. 


#include "main.h"
pros::Motor r_drive_motor(1);
pros::Motor l_drive_motor(2);

float triangle_profile_power(int req_distance, int total_ms, int current_time){
 int is_negative = 1;
float average_velocity = (float) (req_distance / total_ms);
float absolute_acceleration = (float) ((max_velocity) / (0.5 * total_ms));
if(current_time >= total_ms/2){
 is_negative = -1;
}
 else is_negative = 1;

return current_time * is_negative * absolute_acceleration;
}

void profile_move(int distance, int total_ms){
 int curr_time = 0;
while(curr_time < total_ms){
 move_chassis(triangle_profile_power(distance, total_ms, curr_time);
current_time += 25;
pros::delay(25);
}
}

void autonomous(){
 profile_move(1000, 2000); // Move the Chassis 1000 encoder units taking 2 seconds
}



Trapezoidal Motion Profile 

The trapezoidal motion profile velocity vs time graph is very similar to the triangular graph. However, the only difference is that there is a “cruise” segment for the max velocity. Here is what it looks like. 


Here are the key equations for the trapezoidal motion profile. 


For this section, we won’t show you the code. Creating a working movement function using trapezoidal motion profile equations is left as an exercise to the reader. 



What We Recommend From Experience 

Slew is significantly easier to write and use than motion profiles. Moreover, slew gets the job done just as well. With this, we recommend actually using slew whenever possible. However, if you do choose to use a motion profile, we recommend having an integral controller at the end of the movement to correct for any error after the motion profile movement.