ank/ES3011
1
Fork 0
Code Releases Activity

full sequence (no tuning or position feedback)

Browse source
This commit is contained in:
Andy Killorin 2025-12-10 01:51:47 -05:00
parent 0fc7ba0f11
commit 5bc480eaab
Signed by: ank
GPG key ID: 23F9463ECB67FE8C
2 changed files with 15 additions and 10 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

5
robot_controller/constants.h
View file

@ -5,7 +5,7 @@ const float VEL_LIMIT = 10.0; // cm/s
const float BACKLASH_RIGHT = 0.0; // degrees const float BACKLASH_RIGHT = 0.0; // degrees
const float BACKLASH_LEFT = 0.0; // degrees const float BACKLASH_LEFT = 0.0; // degrees
const float WHEEL_DIAMETER = 10.00; // cm const float WHEEL_DIAMETER = 10.00; // cm
const float WHEEL_TO_WHEEL = 0.0; // cm const float WHEEL_TO_WHEEL = 10.0; // cm
// centimeters per second to rpm // centimeters per second to rpm
const float CMS_RPM = 60.0 / (PI*WHEEL_DIAMETER); const float CMS_RPM = 60.0 / (PI*WHEEL_DIAMETER);
@ -13,8 +13,9 @@ const float CMS_RPM = 60.0 / (PI*WHEEL_DIAMETER);
// degrees to centimeters // degrees to centimeters
const float DEG_CM = (PI*WHEEL_DIAMETER) / 360.0; const float DEG_CM = (PI*WHEEL_DIAMETER) / 360.0;
const float FORWARD_DISTANCE = 30.0; // cm const float FORWARD_DISTANCE = 100.0; // cm
const float TURN_AMOUNT = 180.0; // degrees const float TURN_AMOUNT = 180.0; // degrees
const float TURN_DISTANCE = (TURN_AMOUNT / 360.0) * WHEEL_TO_WHEEL * PI;
const float RETURN_DISTANCE = 100.0; // cm const float RETURN_DISTANCE = 100.0; // cm
const float FF_ACCEL = 0.7; // motor acceleration feedforward const float FF_ACCEL = 0.7; // motor acceleration feedforward

20
robot_controller/robot_controller.ino
View file

@ -25,6 +25,7 @@ float phase_start;
enum ROBOT_STATE robot_state; enum ROBOT_STATE robot_state;
struct Trapezoidal forward = {VEL_LIMIT, ACCEL_LIMIT, FORWARD_DISTANCE}; struct Trapezoidal forward = {VEL_LIMIT, ACCEL_LIMIT, FORWARD_DISTANCE};
struct Trapezoidal turn = {VEL_LIMIT, ACCEL_LIMIT, TURN_DISTANCE};
void write_rpm_ff(SmartMotor* motor, int32_t rpm, float ff) { void write_rpm_ff(SmartMotor* motor, int32_t rpm, float ff) {
if (rpm == 0) {rpm = 1;}; if (rpm == 0) {rpm = 1;};
@ -57,11 +58,11 @@ void loop() {
float time = (float)millis() / 1000.0 - phase_start; float time = (float)millis() / 1000.0 - phase_start;
switch (robot_state) { switch (robot_state) {
case FORWARD: case FORWARD:
case RETURN:
setpoint = trapezoidal_planner(&forward, time); setpoint = trapezoidal_planner(&forward, time);
break; break;
case TURN: case TURN:
break; setpoint = trapezoidal_planner(&forward, time);
case RETURN:
break; break;
} }
@ -106,17 +107,20 @@ void loop() {
case FORWARD: case FORWARD:
robot_state = TURN; robot_state = TURN;
phase_start = (float)millis() / 1000.0; phase_start = (float)millis() / 1000.0;
break;
case TURN:
robot_state = RETURN;
phase_start = (float)millis() / 1000.0;
// end (temp) break;
case RETURN:
// end
left_motor.write_rpm(0.0); left_motor.write_rpm(0.0);
delay(10); delay(1);
right_motor.write_rpm(0.0); right_motor.write_rpm(0.0);
for (;;) {}; for (;;) {};
break; break;
case TURN:
break;
case RETURN:
break;
} }
} }
} }