distances currently off by PI/2, unknown why

robot_controller/constants.h

@@ -1,11 +1,11 @@
 #pragma once
 
 const float ACCEL_LIMIT = 5.0; // cm/s/s
-const float VEL_LIMIT = 10.0; // cm/s
+const float VEL_LIMIT = 16.0; // cm/s
 const float BACKLASH_RIGHT = 0.0; // degrees
 const float BACKLASH_LEFT = 0.0; // degrees
-const float WHEEL_DIAMETER = 10.00; // cm
-const float WHEEL_TO_WHEEL = 10.0; // cm
+const float WHEEL_DIAMETER = 10.25; // cm
+const float WHEEL_TO_WHEEL = 9.0; // cm
 
 // centimeters per second to rpm
 const float CMS_RPM = 60.0 / (PI*WHEEL_DIAMETER);
@@ -13,7 +13,7 @@ const float CMS_RPM = 60.0 / (PI*WHEEL_DIAMETER);
 // degrees to centimeters
 const float DEG_CM = (PI*WHEEL_DIAMETER) / 360.0;
 
-const float FORWARD_DISTANCE = 100.0; // cm
+const float FORWARD_DISTANCE = 10.0; // cm
 const float TURN_AMOUNT = 180.0; // degrees
 const float TURN_DISTANCE = (TURN_AMOUNT / 360.0) * WHEEL_TO_WHEEL * PI;
 const float RETURN_DISTANCE = 100.0; // cm
@@ -24,3 +24,5 @@ const float FF_STAT = 15.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 KPI = 0.05; // position integral

robot_controller/robot_controller.ino

@@ -19,7 +19,6 @@ enum ROBOT_STATE {
 float CURRENT_POSITION = 0.0;
 float CURRENT_ROTATION = 0.0;
 struct Setpoint setpoint; 
-float velocity = 40.0;
 // start of current state
 float phase_start;
 enum ROBOT_STATE robot_state;
@@ -27,6 +26,13 @@ enum ROBOT_STATE robot_state;
 struct Trapezoidal forward = {VEL_LIMIT, ACCEL_LIMIT, FORWARD_DISTANCE};
 struct Trapezoidal turn = {VEL_LIMIT, ACCEL_LIMIT, TURN_DISTANCE};
 
+float left_iaccum = 0.0;
+float right_iaccum = 0.0;
+float time_p = 0.0;
+
+float left_home = 0.0;
+float right_home = 0.0;
+
 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
@@ -43,15 +49,15 @@ void setup() {
 
     Wire.begin(); // INIT ARDUINO UNO AS I2C CONTROLLER
     
-    // TUNE VELOCITY PID
-    //left_motor.tune_vel_pid(0.9, 3.7,0.3,0.0); 
-    //delay(10);
-    //right_motor.tune_vel_pid(0.9, 3.7,0.3,0.0); 
-    delay(10);
-    //right_motor.set_direction(PIDDirection::DIRECT);
+    left_motor.write_rpm(0.0);
+    delay(1);
+    right_motor.write_rpm(0.0);
+    delay(999);
     
     phase_start = (float)millis() / 1000.0;
     robot_state = FORWARD;
+    left_motor.home();
+    right_motor.home();
 }
 
 void loop() {
@@ -62,29 +68,50 @@ void loop() {
       setpoint = trapezoidal_planner(&forward, time);
       break;
     case TURN:
-      setpoint = trapezoidal_planner(&forward, time);
+      setpoint = trapezoidal_planner(&turn, time);
       break;
   }
 
-  float feedforward = 
+  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();
+
+  // 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;
+  }
+  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_velocity = setpoint.velocity + left_effort;
+  float right_velocity = (turnsig * setpoint.velocity) + right_effort;
+  
+  // calculate feedforward from motion profile and position PI data
+  float feedforward_left = 
     setpoint.acceleration * CMS_RPM * FF_ACCEL + 
-    setpoint.velocity * CMS_RPM * FF_VEL + 
-    ((setpoint.velocity > 0.0) ? FF_STAT : -FF_STAT);
-
+    left_velocity * CMS_RPM * FF_VEL + 
+    ((left_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);
+    setpoint.acceleration * CMS_RPM * FF_ACCEL + 
+    right_velocity * CMS_RPM * FF_VEL + 
+    ((right_velocity > 0.0) ? FF_STAT : -FF_STAT);
 
-  write_rpm_ff(&left_motor, setpoint.velocity * CMS_RPM, feedforward);
-  write_rpm_ff(&right_motor, (robot_state == TURN ? setpoint.velocity : -setpoint.velocity) * CMS_RPM, feedforward);
+  // 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);
 
-  // READ MOTOR POSITION
+  // send telemetry
   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_right);
+  Serial.print(right_effort);
   Serial.print(",time:");
   Serial.print(time);
   Serial.print(",distgoal:");
@@ -101,8 +128,16 @@ void loop() {
   Serial.print(error / CMS_RPM);
   Serial.println("");
 
-  // move on if at setpoint TODO: check that the error is low as well
-  if (setpoint.complete) {
+  // 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) {
+    // rehome
+    left_home += (setpoint.position / DEG_CM);
+    right_home += (turnsig * setpoint.position / DEG_CM);
+    // zero accumulators
+    right_iaccum = 0.0; left_iaccum = 0.0;
+
     switch (robot_state) {
       case FORWARD:
         robot_state = TURN;