ank/ES3011
1
Fork 0
Code Releases Activity

factored out trapezoidal config to struct

Browse source
This commit is contained in:
Andy Killorin 2025-12-06 14:37:10 -05:00
parent fbea26a946
commit 81bec195f0
Signed by: ank
GPG key ID: 23F9463ECB67FE8C
2 changed files with 69 additions and 15 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

50
robot_controller/robot_controller.ino
View file

@ -17,6 +17,23 @@ const float RETURN_DISTANCE = 100.0; // cm
SmartMotor left_motor(0x0A);
SmartMotor right_motor(0x0B);
enum ROBOT_STATE {
FORWARD,
TURN,
RETURN,
COMPLETE,
};
float CURRENT_POSITION = 0.0;
float CURRENT_ROTATION = 0.0;
struct Setpoint setpoint;
float velocity = 40.0;
// start of current state
float phase_start;
enum ROBOT_STATE robot_state;
struct Trapezoidal forward = {VEL_LIMIT, ACCEL_LIMIT, FORWARD_DISTANCE};
void setup() {
// INIT SERIAL
Serial.begin(115200);
@ -39,25 +56,16 @@ void setup() {
Serial.print("Pos: ");
Serial.print(pos);
Serial.println(" deg");
phase_start = (float)millis() / 1000.0;
robot_state = FORWARD;
}
enum ROBOT_STATE {
FORWARD,
TURN,
RETURN,
COMPLETE,
};
enum ROBOT_STATE robot_state = FORWARD;
float CURRENT_POSITION = 0.0;
float CURRENT_ROTATION = 0.0;
float velocity = 40.0;
void loop() {
float time = (float)millis() / 1000.0;
switch (robot_state) {
case FORWARD:
setpoint = trapezoidal_planner(&forward, time);
break;
case TURN:
break;
@ -81,4 +89,18 @@ void loop() {
Serial.print(" Err: ");
Serial.print(error);
Serial.println("");
// move on if at setpoint TODO: check that the error is low as well
if (setpoint.completed) {
switch (robot_state) {
case FORWARD:
robot_state = TURN;
phase_start = (float)millis() / 1000.0;
break;
case TURN:
break;
case RETURN:
break;
}
}
}

34
robot_controller/trapezoidal.ino
View file

@ -1,5 +1,12 @@
// trapezoidal impl, not fuzzed
// unitless trapezoidal
struct Trapezoidal {
float max_vel;
float max_acc;
float dist;
};
struct Setpoint {
float velocity; // unitless
float position; // unitless
@ -9,7 +16,11 @@ 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(struct Trapezoidal* trapezoidal, float time) {
float max_vel = trapezoidal->max_vel;
float max_acc = trapezoidal->max_acc;
float dist = trapezoidal->dist;
struct Setpoint setpoint = {0.0, 0.0};
float time_accelerating = max_acc / max_vel;
@ -66,3 +77,24 @@ struct Setpoint trapezoidal_planner(float max_vel, float max_acc, float dist, fl
return setpoint;
}
float trapezoidal_time(struct Trapezoidal* trapezoidal) {
float max_vel = trapezoidal->max_vel;
float max_acc = trapezoidal->max_acc;
float dist = trapezoidal->dist;
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);
return peak_velocity_time * 2.0;
} 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;
return total_time;
}
}