RIT VEXU Core API
|
#include <trapezoid_profile.h>
Public Member Functions | |
TrapezoidProfile (double max_v, double accel) | |
Construct a new Trapezoid Profile object. | |
motion_t | calculate (double time_s) |
Run the trapezoidal profile based on the time that's ellapsed. | |
void | set_endpts (double start, double end) |
void | set_accel (double accel) |
void | set_max_v (double max_v) |
double | get_movement_time () |
Trapezoid Profile
This is a motion profile defined by an acceleration, maximum velocity, start point and end point. Using this information, a parametric function is generated, with a period of acceleration, constant velocity, and deceleration. The velocity graph looks like a trapezoid, giving it it's name.
If the maximum velocity is set high enough, this will become a S-curve profile, with only acceleration and deceleration.
This class is designed for use in properly modelling the motion of the robots to create a feedfoward and target for PID. Acceleration and Maximum velocity should be measured on the robot and tuned down slightly to account for battery drop.
Here are the equations graphed for ease of understanding: https://www.desmos.com/calculator/rkm3ivu1yk
TrapezoidProfile::TrapezoidProfile | ( | double | max_v, |
double | accel ) |
Construct a new Trapezoid Profile object.
max_v | Maximum velocity the robot can run at |
accel | Maximum acceleration of the robot |
motion_t TrapezoidProfile::calculate | ( | double | time_s | ) |
Run the trapezoidal profile based on the time that's ellapsed.
time_s | Time since start of movement |
double TrapezoidProfile::get_movement_time | ( | ) |
uses the kinematic equations to and specified accel and max_v to figure out how long moving along the profile would take
void TrapezoidProfile::set_accel | ( | double | accel | ) |
set_accel sets the acceleration this profile will use (the left and right legs of the trapezoid)
accel | the acceleration amount to use |
void TrapezoidProfile::set_endpts | ( | double | start, |
double | end ) |
set_endpts defines a start and end position
start | the starting position of the path |
end | the ending position of the path |
void TrapezoidProfile::set_max_v | ( | double | max_v | ) |
sets the maximum velocity for the profile (the height of the top of the trapezoid)
max_v | the maximum velocity the robot can travel at |