dead code removal

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 ACCEL_LIMIT = 6.0; // cm/s/s
-const float VEL_LIMIT = 16.0; // cm/s
+const float VEL_LIMIT = 12.0; // cm/s
-const float BACKLASH_RIGHT = 0.0; // degrees
+const float WHEEL_DIAMETER = 6.61; // cm
-const float BACKLASH_LEFT = 0.0; // degrees
+const float WHEEL_TO_WHEEL = 8.6; // 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,16 +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 = 10.0; // cm
+const float FORWARD_DISTANCE = 100.0; // cm
-const float TURN_AMOUNT = 175.0; // degrees
+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.3; // motor acceleration feedforward
+const float KP = 1.50; // proportional
-const float FF_VEL = 0.7; // motor velocity feedforward
+const float KI = 0.12; // integral
-const float FF_STAT = 16.4; // motor static friction
+const float KD = 0.00; // derivative
-const float KP = 2.0; // proportional
+const float Kv = 1.85; // onboard velocity feedforward
-const float KI = 0.5; // integral
+
-const float KD = 0.05; // derivative
+const float TURN_KP = 1.50; // proportional
-const float KPP = 0.25; // position proportional
+const float TURN_KI = 0.10; // integral
-const float KPI = 0.05; // position 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

@@ -16,8 +16,6 @@ enum ROBOT_STATE {
   COMPLETE,
 };
 
-float CURRENT_POSITION = 0.0;
-float CURRENT_ROTATION = 0.0;
 struct Setpoint setpoint; 
 // start of current state
 float phase_start;
@@ -33,26 +31,32 @@ 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
-  motor->tune_vel_pid(kV, KP,KI,KD);
-  delay(1);
-  motor->write_rpm(rpm);
-  delay(1);
-}
-
 void setup() {
     // INIT SERIAL
     Serial.begin(115200);
     while(!Serial);
 
     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);
+		right_motor.tune_pos_pid(0.4, 0.024, 0.008);
-    delay(999);
+    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 +85,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;
@@ -95,46 +102,52 @@ void loop() {
   float right_velocity = setpoint.velocity + right_effort;
   float left_velocity = (turnsig * setpoint.velocity) + left_effort;
   
-  // calculate feedforward from motion profile and position PI data
+	if (setpoint.complete && robot_state == TURN) {
-  float feedforward_right = 
+		right_motor.write_angle((setpoint.position / DEG_CM) + right_home);
-    setpoint.acceleration * CMS_RPM * FF_ACCEL + 
+		left_motor.write_angle((turnsig * setpoint.position / DEG_CM) + left_home);
-    right_velocity * CMS_RPM * FF_VEL + 
+	} else {
-    ((right_velocity > 0.0) ? FF_STAT : -FF_STAT);
+    right_motor.write_rpm(right_velocity * CMS_RPM);
-  float feedforward_left = 
+    delay(1);
-    setpoint.acceleration * CMS_RPM * FF_ACCEL + 
+    left_motor.write_rpm(left_velocity * CMS_RPM);
-    left_velocity * CMS_RPM * FF_VEL + 
+    delay(1);
-    ((left_velocity > 0.0) ? FF_STAT : -FF_STAT);
+	}
-
-  // arbitrary feedforward with on-board velocity PID
-  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 = 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;
-  Serial.print("ff:");
+  Serial.print("el:");
-  Serial.print(left_effort);
+  Serial.print(pos_err_left);
-  Serial.print(",time:");
+  Serial.print(",er:");
-  Serial.print(time);
+  Serial.print(pos_err_right);
-  Serial.print(",distgoal:");
+  Serial.print(",il:");
-  Serial.print(setpoint.position);
+  Serial.print(left_iaccum);
-  Serial.print(",dist:");
+  Serial.print(",ir:");
-  Serial.print(pos * DEG_CM);
+  Serial.print(right_iaccum);
-  Serial.print(",cms/s:");
+  Serial.print(",effr:");
+  Serial.print(right_effort);
+  //Serial.print("ff:");
+  //Serial.print(left_effort);
+  //Serial.print(",time:");
+  //Serial.print(time);
+  //Serial.print(",distgoal:");
+  //Serial.print(setpoint.position);
+  //Serial.print(",dist:");
+  //Serial.print(pos * DEG_CM);
+  Serial.print(",vel:");
   Serial.print(rpm / CMS_RPM);
   Serial.print(",setvel:");
   Serial.print(setpoint.velocity);
-  Serial.print(",setacc:");
+  Serial.print(",rvel:");
-  Serial.print(setpoint.acceleration);
+  Serial.print(right_velocity);
-  Serial.print(",Err:");
+  //Serial.print(",setacc:");
-  Serial.print(error / CMS_RPM);
+  //Serial.print(setpoint.acceleration);
+  //Serial.print(",Err:");
+  //Serial.print(error / CMS_RPM);
   Serial.println("");
 
-  // total wheel offness in degrees
+  // move on if at setpoint or timeout
-  float total_pos_error = abs(pos_err_left) + abs(pos_err_right);
+  if (setpoint.complete && (total_pos_error < 3.0 || time - end_time > 5.0)) {
-  // move on if at setpoint TODO: give up after timeout
-  if (setpoint.complete && (total_pos_error < 3.0 || time - end_time > 3.0)) {
     // rehome
     right_home += (setpoint.position / DEG_CM);
     left_home += (turnsig * setpoint.position / DEG_CM);
@@ -143,10 +156,21 @@ void loop() {
 
     switch (robot_state) {
       case FORWARD:
+        // turn tuning (more aggressive)
+        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 (less aggressive)
+        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);