ank/ES3011
1
Fork 0
Code Releases Activity

floundering

Browse source
This commit is contained in:
Andy Killorin 2025-12-10 18:22:03 -05:00
parent 7e6ba786eb
commit 2aa25f49d2
Signed by: ank
GPG key ID: 23F9463ECB67FE8C
2 changed files with 31 additions and 18 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

20
robot_controller/constants.h
View file

@ -13,18 +13,18 @@ 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 = 73.0; // cm //const float FORWARD_DISTANCE = 73.0; // cm
//const float FORWARD_DISTANCE = 10.0; // cm const float FORWARD_DISTANCE = 10.0; // cm
const float TURN_AMOUNT = 146.0; // degrees const float TURN_AMOUNT = 146.0; // degrees
const float TURN_DISTANCE = (TURN_AMOUNT / 360.0) * WHEEL_TO_WHEEL * PI; 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 = 2.4; // motor acceleration feedforward const float FF_ACCEL = 5.4; // motor acceleration feedforward
const float FF_VEL = 0.9; // motor velocity feedforward const float FF_VEL = 0.9; // motor velocity feedforward
const float FF_STAT = 16.4; // motor static friction const float FF_STAT = 18.4; // motor static friction
const float KP = 2.9; // proportional const float KP = 9.13; // proportional
const float KI = 0.8; // integral const float KI = 0.0; // integral
const float KD = 0.05; // derivative const float KD = 0.0; // derivative
const float KPP = 0.25; // position proportional const float KPP = 0.0; // position proportional
const float KPI = 0.17; // position integral const float KPI = 0.0; // position integral
const float KRP = 0.07; // rotation proportional const float KRP = 0.0; // rotation proportional

29
robot_controller/robot_controller.ino
View file

@ -48,11 +48,19 @@ void setup() {
while(!Serial); while(!Serial);
Wire.begin(); // INIT ARDUINO UNO AS I2C CONTROLLER Wire.begin(); // INIT ARDUINO UNO AS I2C CONTROLLER
//left_motor.tune_pos_pid(0.4, 0.024, 0.008);
//right_motor.tune_pos_pid(0.4, 0.024, 0.008);
//delay(1000);
//left_motor.home();
//left_motor.write_angle(180);
//delay(3000);
left_motor.write_rpm(0.0); left_motor.write_rpm(0.0);
delay(1); delay(1);
right_motor.write_rpm(0.0); right_motor.write_rpm(0.0);
delay(2999); //delay(2999);
phase_start = (float)millis() / 1000.0; phase_start = (float)millis() / 1000.0;
robot_state = FORWARD; robot_state = FORWARD;
@ -106,7 +114,7 @@ void loop() {
((left_velocity > 0.0) ? FF_STAT : -FF_STAT); ((left_velocity > 0.0) ? FF_STAT : -FF_STAT);
// arbitrary feedforward with on-board velocity PID // arbitrary feedforward with on-board velocity PID
if (setpoint.complete) { if (setpoint.complete && 0) {
right_motor.write_angle((setpoint.position / DEG_CM) + right_home); right_motor.write_angle((setpoint.position / DEG_CM) + right_home);
left_motor.write_angle((turnsig * setpoint.position / DEG_CM) + left_home); left_motor.write_angle((turnsig * setpoint.position / DEG_CM) + left_home);
} else { } else {
@ -134,12 +142,12 @@ void loop() {
//Serial.print(setpoint.position); //Serial.print(setpoint.position);
//Serial.print(",dist:"); //Serial.print(",dist:");
//Serial.print(pos * DEG_CM); //Serial.print(pos * DEG_CM);
//Serial.print(",cms/s:"); Serial.print(",vel:");
//Serial.print(rpm / CMS_RPM); Serial.print(rpm / CMS_RPM);
//Serial.print(",setvel:"); Serial.print(",setvel:");
//Serial.print(setpoint.velocity); Serial.print(setpoint.velocity);
//Serial.print(",setacc:"); Serial.print(",setacc:");
//Serial.print(setpoint.acceleration); Serial.print(setpoint.acceleration);
//Serial.print(",Err:"); //Serial.print(",Err:");
//Serial.print(error / CMS_RPM); //Serial.print(error / CMS_RPM);
Serial.println(""); Serial.println("");
@ -159,6 +167,11 @@ void loop() {
robot_state = TURN; robot_state = TURN;
right_iaccum = left_iaccum = 8.0; right_iaccum = left_iaccum = 8.0;
phase_start = (float)millis() / 1000.0; phase_start = (float)millis() / 1000.0;
// end
left_motor.write_rpm(0.0);
delay(1);
right_motor.write_rpm(0.0);
for (;;) {};
break; break;
case TURN: case TURN:
robot_state = RETURN; robot_state = RETURN;