ank/ES3011
1
Fork 0
Code Releases Activity

attempt to achieve left-right parity (very different)

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

2
robot_controller/constants.h
View file

@ -1,7 +1,7 @@
#pragma once #pragma once
const float ACCEL_LIMIT = 5.0; // cm/s/s const float ACCEL_LIMIT = 5.0; // cm/s/s
const float VEL_LIMIT = 15.0; // cm/s 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

25
robot_controller/robot_controller.ino
View file

@ -30,7 +30,9 @@ void write_rpm_ff(SmartMotor* motor, int32_t rpm, float ff) {
if (rpm == 0) {rpm = 1;}; if (rpm == 0) {rpm = 1;};
float kV = ff / (float) rpm; // calculate velocity feedforward that causes desired absolute feedforward float kV = ff / (float) rpm; // calculate velocity feedforward that causes desired absolute feedforward
motor->tune_vel_pid(kV, KP,KI,KD); motor->tune_vel_pid(kV, KP,KI,KD);
delay(1);
motor->write_rpm(rpm); motor->write_rpm(rpm);
delay(1);
} }
void setup() { void setup() {
@ -47,14 +49,6 @@ void setup() {
delay(10); delay(10);
//right_motor.set_direction(PIDDirection::DIRECT); //right_motor.set_direction(PIDDirection::DIRECT);
delay(1000);
// READ MOTOR POSITION
int32_t pos = left_motor.read_angle();
Serial.print("Pos: ");
Serial.print(pos);
Serial.println(" deg");
phase_start = (float)millis() / 1000.0; phase_start = (float)millis() / 1000.0;
robot_state = FORWARD; robot_state = FORWARD;
} }
@ -76,17 +70,20 @@ void loop() {
setpoint.velocity * CMS_RPM * FF_VEL + setpoint.velocity * CMS_RPM * FF_VEL +
((setpoint.velocity > 0.0) ? FF_STAT : -FF_STAT); ((setpoint.velocity > 0.0) ? FF_STAT : -FF_STAT);
float feedforward_right =
setpoint.acceleration * CMS_RPM * 9.0 +
setpoint.velocity * CMS_RPM * FF_VEL +
((setpoint.velocity > 0.0) ? FF_STAT : -FF_STAT);
write_rpm_ff(&left_motor, setpoint.velocity * CMS_RPM, feedforward); write_rpm_ff(&left_motor, setpoint.velocity * CMS_RPM, feedforward);
delay(1); write_rpm_ff(&right_motor, (robot_state == TURN ? setpoint.velocity : -setpoint.velocity) * CMS_RPM, feedforward);
//write_rpm_ff(&right_motor, (robot_state == TURN ? velocity : -velocity) * CMS_RPM, feedforward);
delay(19);
// READ MOTOR POSITION // READ MOTOR POSITION
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("ff:"); Serial.print("ff:");
Serial.print(feedforward); Serial.print(feedforward_right);
Serial.print(",time:"); Serial.print(",time:");
Serial.print(time); Serial.print(time);
Serial.print(",distgoal:"); Serial.print(",distgoal:");