factored out trapezoidal calculations
This commit is contained in:
parent
f956c51d83
commit
fbea26a946
GPG key ID:
23F9463ECB67FE8C
2 changed files with 68 additions and 66 deletions
|
|
@ -54,72 +54,6 @@ float CURRENT_POSITION = 0.0;
|
|||
float CURRENT_ROTATION = 0.0;
|
||||
float velocity = 40.0;
|
||||
|
||||
struct Setpoint {
|
||||
float velocity; // unitless
|
||||
float position; // unitless
|
||||
bool complete; // the setpoint will no longer change
|
||||
};
|
||||
|
||||
// returns the position and velocity at the given time on a trapezoidal motion plan
|
||||
// this could be baked if too computationally expensive
|
||||
// not fully fuzzed
|
||||
struct Setpoint trapezoidal_planner(float max_vel, float max_acc, float dist, float time) {
|
||||
struct Setpoint setpoint = {0.0, 0.0};
|
||||
|
||||
float time_accelerating = max_acc / max_vel;
|
||||
float distance_while_accelerating = 0.5 * max_acc * time_accelerating * time_accelerating;
|
||||
|
||||
if (2.0 * distance_while_accelerating > dist) {
|
||||
// triangular
|
||||
float peak_velocity_time = sqrt(dist/max_acc);
|
||||
float peak_velocity = max_acc * peak_velocity_time;
|
||||
|
||||
if (time < peak_velocity_time) {
|
||||
// accelerating
|
||||
setpoint.velocity = max_acc * time;
|
||||
setpoint.position = 0.5 * max_acc * time * time;
|
||||
} else if (time < peak_velocity_time * 2.0) {
|
||||
// slowing down
|
||||
float time_decay = time - peak_velocity_time;
|
||||
setpoint.velocity = peak_velocity - (time_decay * max_acc);
|
||||
setpoint.position = (0.5 * max_acc * peak_velocity_time * peak_velocity_time) // acceleration phase
|
||||
+ peak_velocity * time_decay
|
||||
- 0.5 * max_acc * time_decay * time_decay;
|
||||
} else {
|
||||
setpoint.velocity = 0;
|
||||
setpoint.position = dist;
|
||||
setpoint.complete = true;
|
||||
}
|
||||
} else {
|
||||
// trapezoidal
|
||||
float cruise_distance = dist - 2.0 * distance_while_accelerating;
|
||||
float cruise_time = cruise_distance / max_vel;
|
||||
float total_time = 2 * time_accelerating + cruise_time;
|
||||
if (time < time_accelerating) {
|
||||
// still accelerating
|
||||
setpoint.velocity = time * max_acc;
|
||||
setpoint.position = 0.5 * max_acc * time * time;
|
||||
} else if (time < time_accelerating + cruise_time) {
|
||||
// cruising
|
||||
setpoint.velocity = max_vel;
|
||||
setpoint.position = distance_while_accelerating + max_vel * (time - time_accelerating);
|
||||
} else if (time < total_time) {
|
||||
// slowing down
|
||||
float time_decay = time - (time_accelerating + cruise_time);
|
||||
setpoint.velocity = max_vel - time_decay * max_acc;
|
||||
setpoint.position = distance_while_accelerating + cruise_distance
|
||||
+ (max_vel * time_decay)
|
||||
- 0.5 * max_acc * time_decay * time_decay;
|
||||
} else {
|
||||
//done
|
||||
setpoint.velocity = 0.0;
|
||||
setpoint.position = dist;
|
||||
setpoint.complete = true;
|
||||
}
|
||||
}
|
||||
|
||||
return setpoint;
|
||||
}
|
||||
|
||||
void loop() {
|
||||
switch (robot_state) {
|
||||
|
|
|
|||
68
robot_controller/trapezoidal.ino
Normal file
68
robot_controller/trapezoidal.ino
Normal file
|
|
@ -0,0 +1,68 @@
|
|||
// trapezoidal impl, not fuzzed
|
||||
|
||||
struct Setpoint {
|
||||
float velocity; // unitless
|
||||
float position; // unitless
|
||||
bool complete; // the setpoint will no longer change
|
||||
};
|
||||
|
||||
// returns the position and velocity at the given time on a trapezoidal motion plan
|
||||
// this could be baked if too computationally expensive
|
||||
// not fully fuzzed
|
||||
struct Setpoint trapezoidal_planner(float max_vel, float max_acc, float dist, float time) {
|
||||
struct Setpoint setpoint = {0.0, 0.0};
|
||||
|
||||
float time_accelerating = max_acc / max_vel;
|
||||
float distance_while_accelerating = 0.5 * max_acc * time_accelerating * time_accelerating;
|
||||
|
||||
if (2.0 * distance_while_accelerating > dist) {
|
||||
// triangular
|
||||
float peak_velocity_time = sqrt(dist/max_acc);
|
||||
float peak_velocity = max_acc * peak_velocity_time;
|
||||
|
||||
if (time < peak_velocity_time) {
|
||||
// accelerating
|
||||
setpoint.velocity = max_acc * time;
|
||||
setpoint.position = 0.5 * max_acc * time * time;
|
||||
} else if (time < peak_velocity_time * 2.0) {
|
||||
// slowing down
|
||||
float time_decay = time - peak_velocity_time;
|
||||
setpoint.velocity = peak_velocity - (time_decay * max_acc);
|
||||
setpoint.position = (0.5 * max_acc * peak_velocity_time * peak_velocity_time) // acceleration phase
|
||||
+ peak_velocity * time_decay
|
||||
- 0.5 * max_acc * time_decay * time_decay;
|
||||
} else {
|
||||
setpoint.velocity = 0;
|
||||
setpoint.position = dist;
|
||||
setpoint.complete = true;
|
||||
}
|
||||
} else {
|
||||
// trapezoidal
|
||||
float cruise_distance = dist - 2.0 * distance_while_accelerating;
|
||||
float cruise_time = cruise_distance / max_vel;
|
||||
float total_time = 2 * time_accelerating + cruise_time;
|
||||
if (time < time_accelerating) {
|
||||
// still accelerating
|
||||
setpoint.velocity = time * max_acc;
|
||||
setpoint.position = 0.5 * max_acc * time * time;
|
||||
} else if (time < time_accelerating + cruise_time) {
|
||||
// cruising
|
||||
setpoint.velocity = max_vel;
|
||||
setpoint.position = distance_while_accelerating + max_vel * (time - time_accelerating);
|
||||
} else if (time < total_time) {
|
||||
// slowing down
|
||||
float time_decay = time - (time_accelerating + cruise_time);
|
||||
setpoint.velocity = max_vel - time_decay * max_acc;
|
||||
setpoint.position = distance_while_accelerating + cruise_distance
|
||||
+ (max_vel * time_decay)
|
||||
- 0.5 * max_acc * time_decay * time_decay;
|
||||
} else {
|
||||
//done
|
||||
setpoint.velocity = 0.0;
|
||||
setpoint.position = dist;
|
||||
setpoint.complete = true;
|
||||
}
|
||||
}
|
||||
|
||||
return setpoint;
|
||||
}
|
||||
Loading…
Reference in a new issue