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
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_LEFT = 0.0; // degrees
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;};
float kV = ff / (float) rpm; // calculate velocity feedforward that causes desired absolute feedforward
motor->tune_vel_pid(kV, KP,KI,KD);
delay(1);
motor->write_rpm(rpm);
delay(1);
}
void setup() {
@ -47,14 +49,6 @@ void setup() {
delay(10);
//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;
robot_state = FORWARD;
}
@ -76,17 +70,20 @@ void loop() {
setpoint.velocity * CMS_RPM * FF_VEL +
((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);
delay(1);
//write_rpm_ff(&right_motor, (robot_state == TURN ? velocity : -velocity) * CMS_RPM, feedforward);
delay(19);
write_rpm_ff(&right_motor, (robot_state == TURN ? setpoint.velocity : -setpoint.velocity) * CMS_RPM, feedforward);
// READ MOTOR POSITION
int32_t rpm = left_motor.read_rpm();
int32_t pos = left_motor.read_angle();
int32_t rpm = -right_motor.read_rpm();
int32_t pos = -right_motor.read_angle();
int32_t error = setpoint.velocity * CMS_RPM - rpm;
Serial.print("ff:");
Serial.print(feedforward);
Serial.print(feedforward_right);
Serial.print(",time:");
Serial.print(time);
Serial.print(",distgoal:");