feedforward tuning first round

robot_controller/constants.h

@@ -16,3 +16,10 @@ const float DEG_CM = (PI*WHEEL_DIAMETER) / 360.0;
 const float FORWARD_DISTANCE = 30.0; // cm
 const float TURN_AMOUNT = 180.0; // degrees
 const float RETURN_DISTANCE = 100.0; // cm
+
+const float FF_ACCEL = 3.0; // motor acceleration feedforward
+const float FF_VEL = 3.9; // motor velocity feedforward
+const float FF_STAT = 2.4; // motor static friction
+const float KP = 3.0; // proportional
+const float KI = 0.0; // integral
+const float KD = 0.0; // derivative

robot_controller/robot_controller.ino

@@ -26,6 +26,13 @@ enum ROBOT_STATE robot_state;
 
 struct Trapezoidal forward = {VEL_LIMIT, ACCEL_LIMIT, FORWARD_DISTANCE};
 
+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);
+  motor->write_rpm(rpm);
+}
+
 void setup() {
     // INIT SERIAL
     Serial.begin(115200);
@@ -34,9 +41,9 @@ 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); 
+    //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);
     
@@ -64,18 +71,23 @@ void loop() {
       break;
   }
 
+  float feedforward = 
+    setpoint.acceleration * CMS_RPM * FF_ACCEL + 
+    setpoint.velocity * CMS_RPM * FF_VEL + 
+    ((setpoint.velocity > 0.0) ? FF_STAT : -FF_STAT);
 
-
-  left_motor.write_rpm(setpoint.velocity * CMS_RPM);
-  delay(10);
-  right_motor.write_rpm((robot_state == TURN ? velocity : -velocity) * CMS_RPM);
-  delay(10);
+  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);
 
   // READ MOTOR POSITION
-  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("time:");
+  Serial.print("ff:");
+  Serial.print(feedforward);
+  Serial.print(",time:");
   Serial.print(time);
   Serial.print(",distgoal:");
   Serial.print(setpoint.position);
@@ -83,8 +95,10 @@ void loop() {
   Serial.print(pos * DEG_CM);
   Serial.print(",cms/s:");
   Serial.print(rpm / CMS_RPM);
-  Serial.print(",Setpoint:");
+  Serial.print(",setvel:");
   Serial.print(setpoint.velocity);
+  Serial.print(",setacc:");
+  Serial.print(setpoint.acceleration);
   Serial.print(",Err:");
   Serial.print(error / CMS_RPM);
   Serial.println("");

robot_controller/trapezoidal.h

@@ -8,6 +8,7 @@ struct Trapezoidal {
 };
 
 struct Setpoint {
+  float acceleration; // unitless
   float velocity; // unitless
   float position; // unitless
   bool complete; // the setpoint will no longer change

robot_controller/trapezoidal.ino

@@ -6,7 +6,7 @@ struct Setpoint trapezoidal_planner(struct Trapezoidal* trapezoidal, float time)
   float max_acc = trapezoidal->max_acc;
   float dist = trapezoidal->dist;
 
-  struct Setpoint setpoint = {0.0, 0.0};
+  struct Setpoint setpoint = {0.0, 0.0, 0.0};
 
   float time_accelerating = max_vel / max_acc;
   float distance_while_accelerating = 0.5 * max_acc * time_accelerating * time_accelerating;
@@ -18,16 +18,19 @@ struct Setpoint trapezoidal_planner(struct Trapezoidal* trapezoidal, float time)
 
     if (time < peak_velocity_time) {
       // accelerating
+      setpoint.acceleration = max_acc;
       setpoint.velocity = max_acc * time;
       setpoint.position = 0.5 * max_acc * time * time;
     } else if (time < peak_velocity_time * 2.0) {
       // slowing down
       float time_decay = time - peak_velocity_time;
+      setpoint.acceleration = -max_acc;
       setpoint.velocity = peak_velocity - (time_decay * max_acc);
       setpoint.position = (0.5 * max_acc * peak_velocity_time * peak_velocity_time) // acceleration phase
         + peak_velocity * time_decay 
         - 0.5 * max_acc * time_decay * time_decay;
     } else {
+      setpoint.acceleration = 0.0;
       setpoint.velocity = 0;
       setpoint.position = dist;
       setpoint.complete = true;
@@ -39,14 +42,17 @@ struct Setpoint trapezoidal_planner(struct Trapezoidal* trapezoidal, float time)
     float total_time = 2 * time_accelerating + cruise_time;
     if (time < time_accelerating) {
       // still accelerating
+      setpoint.acceleration = max_acc;
       setpoint.velocity = time * max_acc;
       setpoint.position = 0.5 * max_acc * time * time;
     } else if (time < time_accelerating + cruise_time) {
       // cruising
+      setpoint.acceleration = 0.0;
       setpoint.velocity = max_vel;
       setpoint.position = distance_while_accelerating + max_vel * (time - time_accelerating);
     } else if (time < total_time) {
       // slowing down
+      setpoint.acceleration = -max_acc;
       float time_decay = time - (time_accelerating + cruise_time);
       setpoint.velocity = max_vel - (time_decay * max_acc);
       setpoint.position = distance_while_accelerating + cruise_distance
@@ -54,6 +60,7 @@ struct Setpoint trapezoidal_planner(struct Trapezoidal* trapezoidal, float time)
         - 0.5 * max_acc * time_decay * time_decay;
     } else {
       //done
+      setpoint.acceleration = 0.0;
       setpoint.velocity = 0.0;
       setpoint.position = dist;
       setpoint.complete = true;