ank/ES3011
1
Fork 0
Code Releases Activity

ffless tune

Browse source
This commit is contained in:
Andy Killorin 2025-12-12 12:51:48 -05:00
parent 7b7cd3d8e7
commit 4910f99a33
Signed by: ank
GPG key ID: 23F9463ECB67FE8C
2 changed files with 33 additions and 30 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

22
robot_controller/constants.h
View file

@ -1,11 +1,9 @@
#pragma once #pragma once
const float ACCEL_LIMIT = 5.0; // cm/s/s const float ACCEL_LIMIT = 7.0; // cm/s/s
const float VEL_LIMIT = 16.0; // cm/s const float VEL_LIMIT = 8.0; // cm/s
const float BACKLASH_RIGHT = 0.0; // degrees
const float BACKLASH_LEFT = 0.0; // degrees
const float WHEEL_DIAMETER = 6.61; // cm const float WHEEL_DIAMETER = 6.61; // cm
const float WHEEL_TO_WHEEL = 9.0; // cm const float WHEEL_TO_WHEEL = 8.6; // 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);
@ -14,18 +12,14 @@ const float CMS_RPM = 60.0 / (PI*WHEEL_DIAMETER);
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 = 40.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 FF_ACCEL = 5.4; // motor acceleration feedforward const float KP = 1.30; // proportional
const float FF_VEL = 0.9; // motor velocity feedforward const float KI = 0.20; // integral
const float FF_STAT = 18.4; // motor static friction const float KD = 0.01; // derivative
const float KP = 0.00; // proportional const float Kv = 1.5; // onboard velocity feedforward
const float KI = 0.0; // integral
const float KD = 0.0; // derivative
const float Kv = 1.0; // onboard velocity feedforward
const float KPP = 0.0; // position proportional const float KPP = 0.0; // position proportional
const float KPI = 0.0; // position integral const float KPI = 0.0; // position integral
const float KRP = 0.0; // rotation proportional const float KRP = 0.0; // rotation proportional

41
robot_controller/robot_controller.ino
View file

@ -50,19 +50,24 @@ void setup() {
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); left_motor.tune_pos_pid(0.4, 0.024, 0.008);
right_motor.tune_pos_pid(0.4, 0.024, 0.008);
left_motor.tune_vel_pid(Kv, KP,KI,KD);
left_motor.tune_vel_pid(Kv, KP,KI,KD);
//delay(1000);
//left_motor.home();
//left_motor.write_angle(180);
//delay(3000);
left_motor.write_rpm(0.0);
delay(1); delay(1);
right_motor.write_rpm(0.0); right_motor.tune_pos_pid(0.4, 0.024, 0.008);
//delay(2999); delay(1);
left_motor.tune_vel_pid(Kv, KP,KI,KD);
delay(1);
right_motor.tune_vel_pid(Kv, KP,KI,KD);
delay(1);
delay(10);
// runs to mend bad state
left_motor.home();
delay(1);
left_motor.write_angle(0.0);
delay(1);
right_motor.home();
delay(1);
right_motor.write_angle(0.0);
delay(3000);
phase_start = (float)millis() / 1000.0; phase_start = (float)millis() / 1000.0;
robot_state = FORWARD; robot_state = FORWARD;
@ -111,12 +116,14 @@ void loop() {
left_motor.write_angle((turnsig * setpoint.position / DEG_CM) + left_home); left_motor.write_angle((turnsig * setpoint.position / DEG_CM) + left_home);
} else { } else {
right_motor.write_rpm(right_velocity * CMS_RPM); right_motor.write_rpm(right_velocity * CMS_RPM);
delay(1);
left_motor.write_rpm(left_velocity * CMS_RPM); left_motor.write_rpm(left_velocity * CMS_RPM);
delay(1);
} }
// send telemetry // send telemetry
int32_t rpm = left_motor.read_rpm(); int32_t rpm = right_motor.read_rpm();
int32_t pos = left_motor.read_angle(); int32_t pos = right_motor.read_angle();
int32_t error = setpoint.velocity * CMS_RPM - rpm; int32_t error = setpoint.velocity * CMS_RPM - rpm;
Serial.print("el:"); Serial.print("el:");
Serial.print(pos_err_left); Serial.print(pos_err_left);
@ -138,8 +145,10 @@ void loop() {
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(",rvel:");
Serial.print(setpoint.acceleration); Serial.print(right_velocity);
//Serial.print(",setacc:");
//Serial.print(setpoint.acceleration);
//Serial.print(",Err:"); //Serial.print(",Err:");
//Serial.print(error / CMS_RPM); //Serial.print(error / CMS_RPM);
Serial.println(""); Serial.println("");