flip left and right motors (right motor has a lot of backlash)

robot_controller/constants.h

@@ -14,15 +14,15 @@ const float CMS_RPM = 60.0 / (PI*WHEEL_DIAMETER);
 const float DEG_CM = (PI*WHEEL_DIAMETER) / 360.0;
 
 const float FORWARD_DISTANCE = 10.0; // cm
-const float TURN_AMOUNT = 180.0; // degrees
+const float TURN_AMOUNT = 175.0; // degrees
 const float TURN_DISTANCE = (TURN_AMOUNT / 360.0) * WHEEL_TO_WHEEL * PI;
 const float RETURN_DISTANCE = 100.0; // cm
 
-const float FF_ACCEL = 0.7; // motor acceleration feedforward
-const float FF_VEL = 0.6; // motor velocity feedforward
-const float FF_STAT = 15.4; // motor static friction
+const float FF_ACCEL = 2.3; // motor acceleration feedforward
+const float FF_VEL = 0.7; // motor velocity feedforward
+const float FF_STAT = 16.4; // motor static friction
 const float KP = 2.0; // proportional
 const float KI = 0.5; // integral
 const float KD = 0.05; // derivative
-const float KPP = 0.15; // position proportional
+const float KPP = 0.25; // position proportional
 const float KPI = 0.05; // position integral

robot_controller/robot_controller.ino

@@ -62,56 +62,59 @@ void setup() {
 
 void loop() {
   float time = (float)millis() / 1000.0 - phase_start;
+  float end_time;
   switch (robot_state) {
     case FORWARD:
     case RETURN:
       setpoint = trapezoidal_planner(&forward, time);
+      end_time = trapezoidal_time(&forward);
       break;
     case TURN:
       setpoint = trapezoidal_planner(&turn, time);
+      end_time = trapezoidal_time(&turn);
       break;
   }
 
   float turnsig = (robot_state == TURN ? 1.0 : -1.0); // direction of travel for right motor
 
   // position PI controller
-  float pos_err_left = ((setpoint.position / DEG_CM) + left_home) - left_motor.read_angle();
-  float pos_err_right = ((turnsig * setpoint.position / DEG_CM) + right_home) - right_motor.read_angle();
+  float pos_err_right = ((setpoint.position / DEG_CM) + right_home) - right_motor.read_angle();
+  float pos_err_left = ((turnsig * setpoint.position / DEG_CM) + left_home) - left_motor.read_angle();
 
   // delta should be ~4ms
   if (time > time_p) {
     float delta = time - time_p;
-    right_iaccum += pos_err_right * delta;
     left_iaccum += pos_err_left * delta;
+    right_iaccum += pos_err_right * delta;
   }
   time_p = time;
 
-  float left_effort = pos_err_left * KPP + left_iaccum * KPI;
   float right_effort = pos_err_right * KPP + right_iaccum * KPI;
+  float left_effort = pos_err_left * KPP + left_iaccum * KPI;
 
-  float left_velocity = setpoint.velocity + left_effort;
-  float right_velocity = (turnsig * setpoint.velocity) + right_effort;
+  float right_velocity = setpoint.velocity + right_effort;
+  float left_velocity = (turnsig * setpoint.velocity) + left_effort;
   
   // calculate feedforward from motion profile and position PI data
-  float feedforward_left = 
-    setpoint.acceleration * CMS_RPM * FF_ACCEL + 
-    left_velocity * CMS_RPM * FF_VEL + 
-    ((left_velocity > 0.0) ? FF_STAT : -FF_STAT);
   float feedforward_right = 
     setpoint.acceleration * CMS_RPM * FF_ACCEL + 
     right_velocity * CMS_RPM * FF_VEL + 
     ((right_velocity > 0.0) ? FF_STAT : -FF_STAT);
+  float feedforward_left = 
+    setpoint.acceleration * CMS_RPM * FF_ACCEL + 
+    left_velocity * CMS_RPM * FF_VEL + 
+    ((left_velocity > 0.0) ? FF_STAT : -FF_STAT);
 
   // arbitrary feedforward with on-board velocity PID
-  write_rpm_ff(&left_motor, left_velocity * CMS_RPM , feedforward_left);
-  write_rpm_ff(&right_motor, right_velocity * CMS_RPM  , feedforward_right);
+  write_rpm_ff(&right_motor, right_velocity * CMS_RPM , feedforward_right);
+  write_rpm_ff(&left_motor, left_velocity * CMS_RPM  , feedforward_left);
 
   // send telemetry
-  int32_t rpm = -right_motor.read_rpm();
-  int32_t pos = -right_motor.read_angle();
+  int32_t rpm = left_motor.read_rpm();
+  int32_t pos = left_motor.read_angle();
   int32_t error = setpoint.velocity * CMS_RPM - rpm;
   Serial.print("ff:");
-  Serial.print(right_effort);
+  Serial.print(left_effort);
   Serial.print(",time:");
   Serial.print(time);
   Serial.print(",distgoal:");
@@ -131,10 +134,10 @@ void loop() {
   // total wheel offness in degrees
   float total_pos_error = abs(pos_err_left) + abs(pos_err_right);
   // move on if at setpoint TODO: give up after timeout
-  if (setpoint.complete && total_pos_error < 10.0) {
+  if (setpoint.complete && (total_pos_error < 3.0 || time - end_time > 3.0)) {
     // rehome
-    left_home += (setpoint.position / DEG_CM);
-    right_home += (turnsig * setpoint.position / DEG_CM);
+    right_home += (setpoint.position / DEG_CM);
+    left_home += (turnsig * setpoint.position / DEG_CM);
     // zero accumulators
     right_iaccum = 0.0; left_iaccum = 0.0;