full sequence (no tuning or position feedback)

robot_controller/constants.h

@@ -1,11 +1,11 @@
 #pragma once
 
 const float ACCEL_LIMIT = 5.0; // cm/s/s
-const float VEL_LIMIT = 15.0; // cm/s
+const float VEL_LIMIT = 10.0; // cm/s
 const float BACKLASH_RIGHT = 0.0; // degrees
 const float BACKLASH_LEFT = 0.0; // degrees
-const float WHEEL_DIAMETER = 6.37; // cm
-const float WHEEL_TO_WHEEL = 0.0; // cm
+const float WHEEL_DIAMETER = 10.00; // cm
+const float WHEEL_TO_WHEEL = 10.0; // cm
 
 // centimeters per second to rpm
 const float CMS_RPM = 60.0 / (PI*WHEEL_DIAMETER);
@@ -13,13 +13,14 @@ 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 = 30.0; // cm
+const float FORWARD_DISTANCE = 100.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
 
-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
+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 KP = 2.0; // proportional
+const float KI = 0.5; // integral
+const float KD = 0.05; // derivative

robot_controller/robot_controller.ino

@@ -25,12 +25,15 @@ float phase_start;
 enum ROBOT_STATE robot_state;
 
 struct Trapezoidal forward = {VEL_LIMIT, ACCEL_LIMIT, FORWARD_DISTANCE};
+struct Trapezoidal turn = {VEL_LIMIT, ACCEL_LIMIT, TURN_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);
+  delay(1);
   motor->write_rpm(rpm);
+  delay(1);
 }
 
 void setup() {
@@ -47,14 +50,6 @@ void setup() {
     delay(10);
     //right_motor.set_direction(PIDDirection::DIRECT);
     
-    delay(1000);
-
-    // READ MOTOR POSITION
-    int32_t pos = left_motor.read_angle();
-    Serial.print("Pos: ");
-    Serial.print(pos);
-    Serial.println(" deg");
-
     phase_start = (float)millis() / 1000.0;
     robot_state = FORWARD;
 }
@@ -63,11 +58,11 @@ void loop() {
   float time = (float)millis() / 1000.0 - phase_start;
   switch (robot_state) {
     case FORWARD:
+    case RETURN:
       setpoint = trapezoidal_planner(&forward, time);
       break;
     case TURN:
-      break;
-    case RETURN:
+      setpoint = trapezoidal_planner(&forward, time);
       break;
   }
 
@@ -76,17 +71,20 @@ void loop() {
     setpoint.velocity * CMS_RPM * FF_VEL + 
     ((setpoint.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);
+
   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);
+  write_rpm_ff(&right_motor, (robot_state == TURN ? setpoint.velocity : -setpoint.velocity) * CMS_RPM, feedforward);
 
   // READ MOTOR POSITION
-  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("ff:");
-  Serial.print(feedforward);
+  Serial.print(feedforward_right);
   Serial.print(",time:");
   Serial.print(time);
   Serial.print(",distgoal:");
@@ -109,16 +107,19 @@ void loop() {
       case FORWARD:
         robot_state = TURN;
         phase_start = (float)millis() / 1000.0;
-
-        // end (temp)
-        left_motor.write_rpm(0.0);
-        delay(10);
-        right_motor.write_rpm(0.0);
-        for (;;) {};
         break;
       case TURN:
+        robot_state = RETURN;
+        phase_start = (float)millis() / 1000.0;
+
         break;
       case RETURN:
+
+        // end
+        left_motor.write_rpm(0.0);
+        delay(1);
+        right_motor.write_rpm(0.0);
+        for (;;) {};
         break;
     }
   }