robot_controller/constants.h

@@ -1,11 +1,11 @@
 #pragma once
 
 const float ACCEL_LIMIT = 5.0; // cm/s/s
-const float VEL_LIMIT = 16.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 = 10.25; // cm
-const float WHEEL_TO_WHEEL = 9.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,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 = 10.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
@@ -24,5 +24,3 @@ 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,6 +19,7 @@ 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;
@@ -26,13 +27,6 @@ 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
@@ -49,15 +43,15 @@ void setup() {
 
     Wire.begin(); // INIT ARDUINO UNO AS I2C CONTROLLER
     
-    left_motor.write_rpm(0.0);
-    delay(1);
-    right_motor.write_rpm(0.0);
-    delay(999);
+    // 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);
     
     phase_start = (float)millis() / 1000.0;
     robot_state = FORWARD;
-    left_motor.home();
-    right_motor.home();
 }
 
 void loop() {
@@ -68,50 +62,29 @@ void loop() {
       setpoint = trapezoidal_planner(&forward, time);
       break;
     case TURN:
-      setpoint = trapezoidal_planner(&turn, time);
+      setpoint = trapezoidal_planner(&forward, time);
       break;
   }
 
-  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 = 
+  float feedforward = 
     setpoint.acceleration * CMS_RPM * FF_ACCEL + 
-    left_velocity * CMS_RPM * FF_VEL + 
-    ((left_velocity > 0.0) ? FF_STAT : -FF_STAT);
+    setpoint.velocity * CMS_RPM * FF_VEL + 
+    ((setpoint.velocity > 0.0) ? FF_STAT : -FF_STAT);
+
   float feedforward_right = 
-    setpoint.acceleration * CMS_RPM * FF_ACCEL + 
-    right_velocity * CMS_RPM * FF_VEL + 
-    ((right_velocity > 0.0) ? FF_STAT : -FF_STAT);
+    setpoint.acceleration * CMS_RPM * 9.0 + 
+    setpoint.velocity * CMS_RPM * FF_VEL + 
+    ((setpoint.velocity > 0.0) ? FF_STAT : -FF_STAT);
 
-  // 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);
+  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);
 
-  // send telemetry
+  // READ MOTOR POSITION
   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(right_effort);
+  Serial.print(feedforward_right);
   Serial.print(",time:");
   Serial.print(time);
   Serial.print(",distgoal:");
@@ -128,16 +101,8 @@ void loop() {
   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 < 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;
-
+  // move on if at setpoint TODO: check that the error is low as well
+  if (setpoint.complete) {
     switch (robot_state) {
       case FORWARD:
         robot_state = TURN;