final tweaks

ran perfectly with 9 cubes attempt 1

robot_controller/constants.h

@@ -1,11 +1,9 @@
 #pragma once
 
-const float ACCEL_LIMIT = 5.0; // cm/s/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.25; // cm
-const float WHEEL_TO_WHEEL = 9.0; // cm
+const float ACCEL_LIMIT = 6.0; // cm/s/s
+const float VEL_LIMIT = 12.0; // cm/s
+const float WHEEL_DIAMETER = 6.61; // cm
+const float WHEEL_TO_WHEEL = 8.6; // cm
 
 // centimeters per second to rpm
 const float CMS_RPM = 60.0 / (PI*WHEEL_DIAMETER);
@@ -13,17 +11,22 @@ 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 = 73.0; // cm
-//const float FORWARD_DISTANCE = 10.0; // cm
-const float TURN_AMOUNT = 146.0; // degrees
+const float FORWARD_DISTANCE = 100.0; // cm
+const float TURN_AMOUNT = 190.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 = 2.5; // motor acceleration feedforward
-const float FF_VEL = 0.9; // motor velocity feedforward
-const float FF_STAT = 16.4; // motor static friction
-const float KP = 2.7; // proportional
-const float KI = 0.5; // integral
-const float KD = 0.05; // derivative
-const float KPP = 0.25; // position proportional
-const float KPI = 0.07; // position integral
+const float KP = 1.50; // proportional
+const float KI = 0.12; // integral
+const float KD = 0.00; // derivative
+const float Kv = 1.85; // onboard velocity feedforward
+
+const float TURN_KP = 1.50; // proportional
+const float TURN_KI = 0.10; // integral
+const float TURN_KD = 0.00; // derivative
+const float TURN_Kv = 2.6; // onboard velocity feedforward
+
+const float KPP = 0.14; // position proportional
+const float KPI = 0.12; // position integral
+const float KRP = 0.0; // rotation proportional
+const float V_MIN = 5.5; // velocity minimum
+const float V_KMIN = 0.7; // velocity minimum coefficient

robot_controller/robot_controller.ino

@@ -49,10 +49,25 @@ void setup() {
 
     Wire.begin(); // INIT ARDUINO UNO AS I2C CONTROLLER
 
-    left_motor.write_rpm(0.0);
+		left_motor.tune_pos_pid(0.4, 0.024, 0.008);
     delay(1);
-    right_motor.write_rpm(0.0);
-    delay(999);
+		right_motor.tune_pos_pid(0.4, 0.024, 0.008);
+    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;
     robot_state = FORWARD;
@@ -81,6 +96,9 @@ void loop() {
   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();
 
+  // total wheel offness in degrees
+  float total_pos_error = abs(pos_err_left) + abs(pos_err_right);
+
   // delta should be ~4ms
   if (time > time_p) {
     float delta = time - time_p;
@@ -92,36 +110,41 @@ void loop() {
   float right_effort = pos_err_right * KPP + right_iaccum * KPI;
   float left_effort = pos_err_left * KPP + left_iaccum * KPI;
 
+  
+#define sgn(x) ((x) < 0 ? -1 : ((x) > 0 ? 1 : 0))
+  // boost on when slowing down
+  //if (setpoint.velocity < V_MIN && setpoint.acceleration < 0.0 && total_pos_error > 4.0) {
+  //  right_effort += (float )sgn(right_effort) * (setpoint.velocity - V_MIN) * V_KMIN;
+  //  left_effort += (float )sgn(left_effort) * (setpoint.velocity - V_MIN) * V_KMIN;
+  //}
   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_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
-	if (setpoint.complete) {
+	if (setpoint.complete && robot_state == TURN) {
 		right_motor.write_angle((setpoint.position / DEG_CM) + right_home);
 		left_motor.write_angle((turnsig * setpoint.position / DEG_CM) + left_home);
 	} else {
-		write_rpm_ff(&right_motor, right_velocity * CMS_RPM , feedforward_right);
-		write_rpm_ff(&left_motor, left_velocity * CMS_RPM  , feedforward_left);
+    right_motor.write_rpm(right_velocity * CMS_RPM);
+    delay(1);
+    left_motor.write_rpm(left_velocity * CMS_RPM);
+    delay(1);
 	}
 
   // send telemetry
-  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("el:");
   Serial.print(pos_err_left);
   Serial.print(",er:");
   Serial.print(pos_err_right);
+  Serial.print(",il:");
+  Serial.print(left_iaccum);
+  Serial.print(",ir:");
+  Serial.print(right_iaccum);
+  Serial.print(",effr:");
+  Serial.print(right_effort);
   //Serial.print("ff:");
   //Serial.print(left_effort);
   //Serial.print(",time:");
@@ -130,20 +153,20 @@ void loop() {
   //Serial.print(setpoint.position);
   //Serial.print(",dist:");
   //Serial.print(pos * DEG_CM);
-  //Serial.print(",cms/s:");
-  //Serial.print(rpm / CMS_RPM);
-  //Serial.print(",setvel:");
-  //Serial.print(setpoint.velocity);
+  Serial.print(",vel:");
+  Serial.print(rpm / CMS_RPM);
+  Serial.print(",setvel:");
+  Serial.print(setpoint.velocity);
+  Serial.print(",rvel:");
+  Serial.print(right_velocity);
   //Serial.print(",setacc:");
   //Serial.print(setpoint.acceleration);
   //Serial.print(",Err:");
   //Serial.print(error / CMS_RPM);
   Serial.println("");
 
-  // 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 < 3.0 || time - end_time > 7.0)) {
+  if (setpoint.complete && (total_pos_error < 3.0 || time - end_time > 5.0)) {
     // rehome
     right_home += (setpoint.position / DEG_CM);
     left_home += (turnsig * setpoint.position / DEG_CM);
@@ -152,10 +175,21 @@ void loop() {
 
     switch (robot_state) {
       case FORWARD:
+        // turn tuning
+        left_motor.tune_vel_pid(TURN_Kv, TURN_KP,TURN_KI,TURN_KD);
+        delay(1);
+        right_motor.tune_vel_pid(TURN_Kv,TURN_KP,TURN_KI,TURN_KD);
+
         robot_state = TURN;
+				right_iaccum = left_iaccum = 60.0;
         phase_start = (float)millis() / 1000.0;
         break;
       case TURN:
+        // straight line tuning
+        left_motor.tune_vel_pid(Kv, KP,KI,KD);
+        delay(1);
+        right_motor.tune_vel_pid(Kv, KP,KI,KD);
+
         robot_state = RETURN;
         phase_start = (float)millis() / 1000.0;
 

robot_controller/trapezoidal.ino

@@ -11,6 +11,10 @@ struct Setpoint trapezoidal_planner(struct Trapezoidal* trapezoidal, float time)
   float time_accelerating = max_vel / max_acc;
   float distance_while_accelerating = 0.5 * max_acc * time_accelerating * time_accelerating;
 
+	if (time < 0.0) {
+		return setpoint;
+	}
+
   if (2.0 * distance_while_accelerating > dist) {
     // triangular
     float peak_velocity_time = sqrt(dist/max_acc);