ank/ES3011
1
Fork 0
Code Releases Activity

named variables as they should be

Browse source
This commit is contained in:
Andy Killorin 2025-12-06 14:22:56 -05:00
parent 17707897d6
commit f956c51d83
Signed by: ank
GPG key ID: 23F9463ECB67FE8C
1 changed files with 6 additions and 6 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

12
robot_controller/robot_controller.ino
View file

@ -63,7 +63,7 @@ struct Setpoint {
// 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 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;
@ -71,7 +71,7 @@ struct Setpoint trapezoidal_planner(float max_vel, float, max_acc, float dist, f
if (2.0 * distance_while_accelerating > dist) {
// triangular
float peak_velocity_time = sqrt(distance/max_acc);
float peak_velocity_time = sqrt(dist/max_acc);
float peak_velocity = max_acc * peak_velocity_time;
if (time < peak_velocity_time) {
@ -82,7 +82,7 @@ struct Setpoint trapezoidal_planner(float max_vel, float, max_acc, float dist, f
// slowing down
float time_decay = time - peak_velocity_time;
setpoint.velocity = peak_velocity - (time_decay * max_acc);
setpoint.position = (0.5 * A * peak_velocity_time * peak_velocity_time) // acceleration phase
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 {
@ -92,7 +92,7 @@ struct Setpoint trapezoidal_planner(float max_vel, float, max_acc, float dist, f
}
} else {
// trapezoidal
float cruise_distance = distance - 2.0 * distance_while_accelerating;
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) {
@ -103,7 +103,7 @@ struct Setpoint trapezoidal_planner(float max_vel, float, max_acc, float dist, f
// cruising
setpoint.velocity = max_vel;
setpoint.position = distance_while_accelerating + max_vel * (time - time_accelerating);
} else if (time < time_total) {
} else if (time < total_time) {
// slowing down
float time_decay = time - (time_accelerating + cruise_time);
setpoint.velocity = max_vel - time_decay * max_acc;
@ -113,7 +113,7 @@ struct Setpoint trapezoidal_planner(float max_vel, float, max_acc, float dist, f
} else {
//done
setpoint.velocity = 0.0;
setpoint.position = dest;
setpoint.position = dist;
setpoint.complete = true;
}
}