ank/ES3011
1
Fork 0
Code Releases Activity

mended units

Browse source
This commit is contained in:
Andy Killorin 2025-12-06 15:36:25 -05:00
parent a29c852daa
commit 7bcd6e5f64
Signed by: ank
GPG key ID: 23F9463ECB67FE8C
3 changed files with 7 additions and 4 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

3
robot_controller/constants.h
View file

@ -8,5 +8,8 @@ const float WHEEL_DIAMETER = 6.37; // cm
const float WHEEL_TO_WHEEL = 0.0; // cm const float WHEEL_TO_WHEEL = 0.0; // cm
const float FORWARD_DISTANCE = 100.0; // cm const float FORWARD_DISTANCE = 100.0; // cm
// centimeters per second to rpm
const float CMS_RPM = 60.0 / (PI*WHEEL_DIAMETER);
const float TURN_AMOUNT = 180.0; // degrees const float TURN_AMOUNT = 180.0; // degrees
const float RETURN_DISTANCE = 100.0; // cm const float RETURN_DISTANCE = 100.0; // cm

4
robot_controller/robot_controller.ino
View file

@ -53,7 +53,7 @@ void setup() {
} }
void loop() { void loop() {
float time = (float)millis() / 1000.0; float time = (float)millis() / 1000.0 - phase_start;
switch (robot_state) { switch (robot_state) {
case FORWARD: case FORWARD:
setpoint = trapezoidal_planner(&forward, time); setpoint = trapezoidal_planner(&forward, time);
@ -66,7 +66,7 @@ void loop() {
left_motor.write_rpm(velocity); left_motor.write_rpm(setpoint.velocity * CMS_RPM);
//right_motor.write_rpm(robot_state == TURN ? velocity : -velocity); //right_motor.write_rpm(robot_state == TURN ? velocity : -velocity);
delay(20); delay(20);

4
robot_controller/trapezoidal.ino
View file

@ -8,7 +8,7 @@ struct Setpoint trapezoidal_planner(struct Trapezoidal* trapezoidal, float time)
struct Setpoint setpoint = {0.0, 0.0}; struct Setpoint setpoint = {0.0, 0.0};
float time_accelerating = max_acc / max_vel; float time_accelerating = max_vel / max_acc;
float distance_while_accelerating = 0.5 * max_acc * time_accelerating * time_accelerating; float distance_while_accelerating = 0.5 * max_acc * time_accelerating * time_accelerating;
if (2.0 * distance_while_accelerating > dist) { if (2.0 * distance_while_accelerating > dist) {
@ -48,7 +48,7 @@ struct Setpoint trapezoidal_planner(struct Trapezoidal* trapezoidal, float time)
} else if (time < total_time) { } else if (time < total_time) {
// slowing down // slowing down
float time_decay = time - (time_accelerating + cruise_time); float time_decay = time - (time_accelerating + cruise_time);
setpoint.velocity = max_vel - time_decay * max_acc; setpoint.velocity = max_vel - (time_decay * max_acc);
setpoint.position = distance_while_accelerating + cruise_distance setpoint.position = distance_while_accelerating + cruise_distance
+ (max_vel * time_decay) + (max_vel * time_decay)
- 0.5 * max_acc * time_decay * time_decay; - 0.5 * max_acc * time_decay * time_decay;