robot_controller/robot_controller.ino

@@ -3,37 +3,9 @@
 #include <smartmotor.h>
 #include <SMC_gains.h>
 
-const float ACCEL_LIMIT = 4.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 = 6.37; // cm
-const float WHEEL_TO_WHEEL = 0.0; // cm
-
-const float FORWARD_DISTANCE = 100.0; // cm
-const float TURN_AMOUNT = 180.0; // degrees
-const float RETURN_DISTANCE = 100.0; // cm
-
 SmartMotor left_motor(0x0A); 
 SmartMotor right_motor(0x0B); 
 
-enum ROBOT_STATE {
-  FORWARD,
-  TURN,
-  RETURN,
-  COMPLETE,
-};
-
-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;
-
-struct Trapezoidal forward = {VEL_LIMIT, ACCEL_LIMIT, FORWARD_DISTANCE};
-
 void setup() {
     // INIT SERIAL
     Serial.begin(115200);
@@ -42,10 +14,11 @@ void setup() {
     Wire.begin(); // INIT ARDUINO UNO AS I2C CONTROLLER
     
     // TUNE POSITION PID
-    left_motor.tune_vel_pid(9.0, 0.0,0.0,0.0); 
+    left_motor.tune_pos_pid(0.65,0.060,0.065); 
-    right_motor.tune_vel_pid(1.0, 0.65,0.060,0.065); 
+    right_motor.tune_pos_pid(0.65,0.060,0.065); 
     delay(1000);
 
+
     // SET MOTOR POSITION
     //int32_t angle=360;
     //uint8_t status= left_motor.write_angle(angle);
@@ -56,51 +29,32 @@ void setup() {
     Serial.print("Pos: ");
     Serial.print(pos);
     Serial.println(" deg");
-
-    phase_start = (float)millis() / 1000.0;
-    robot_state = FORWARD;
 }
 
+int32_t angle = 0;
+
+enum ROBOT_STATE {
+  FORWARD,
+  TURN,
+  REVERSE
+};
+
+enum ROBOT_STATE robot_state = FORWARD;
+
 void loop() {
-  float time = (float)millis() / 1000.0;
+    angle = (millis() / 2000) * 360;
-  switch (robot_state) {
+    left_motor.write_angle(angle);
-    case FORWARD:
+    right_motor.write_angle(angle);
-      setpoint = trapezoidal_planner(&forward, time);
+    delay(20);
-      break;
-    case TURN:
-      break;
-    case RETURN:
-      break;
-  }
 
-
+    // READ MOTOR POSITION
-
+    int32_t pos = left_motor.read_angle();
-  left_motor.write_rpm(velocity);
+    int32_t error = angle - pos;
-  //right_motor.write_rpm(robot_state == TURN ? velocity : -velocity);
+    Serial.print("Pos: ");
-  delay(20);
+    Serial.print(pos);
-
+    Serial.print(" Setpoint: ");
-  // READ MOTOR POSITION
+    Serial.print(angle);
-  int32_t rpm = left_motor.read_rpm();
+    Serial.print(" Err: ");
-  int32_t error = velocity - rpm;
+    Serial.print(error);
-  Serial.print("RPM: ");
+    Serial.println("");
-  Serial.print(rpm);
-  Serial.print(" Setpoint: ");
-  Serial.print(velocity);
-  Serial.print(" Err: ");
-  Serial.print(error);
-  Serial.println("");
-
-  // move on if at setpoint TODO: check that the error is low as well
-  if (setpoint.completed) {
-    switch (robot_state) {
-      case FORWARD:
-        robot_state = TURN;
-        phase_start = (float)millis() / 1000.0;
-        break;
-      case TURN:
-        break;
-      case RETURN:
-        break;
-    }
-  }
 } 

robot_controller/trapezoidal.ino

@@ -1,100 +0,0 @@
-// trapezoidal impl, not fuzzed
-
-// unitless trapezoidal
-struct Trapezoidal {
-  float max_vel;
-  float max_acc;
-  float dist;
-};
-
-struct Setpoint {
-  float velocity; // unitless
-  float position; // unitless
-  bool complete; // the setpoint will no longer change
-};
-
-// returns the position and velocity at the given time on a trapezoidal motion plan
-// this could be baked if too computationally expensive
-// not fully fuzzed
-struct Setpoint trapezoidal_planner(struct Trapezoidal* trapezoidal, float time) {
-  float max_vel = trapezoidal->max_vel;
-  float max_acc = trapezoidal->max_acc;
-  float dist = trapezoidal->dist;
-
-  struct Setpoint setpoint = {0.0, 0.0};
-
-  float time_accelerating = max_acc / max_vel;
-  float distance_while_accelerating = 0.5 * max_acc * time_accelerating * time_accelerating;
-
-  if (2.0 * distance_while_accelerating > dist) {
-    // triangular
-    float peak_velocity_time = sqrt(dist/max_acc);
-    float peak_velocity = max_acc * peak_velocity_time;
-
-    if (time < peak_velocity_time) {
-      // accelerating
-      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.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.velocity = 0;
-      setpoint.position = dist;
-      setpoint.complete = true;
-    }
-  } else {
-    // trapezoidal
-    float cruise_distance = dist - 2.0 * distance_while_accelerating;
-    float cruise_time = cruise_distance / max_vel;
-    float total_time = 2 * time_accelerating + cruise_time;
-    if (time < time_accelerating) {
-      // still accelerating
-      setpoint.velocity = time * max_acc;
-      setpoint.position = 0.5 * max_acc * time * time;
-    } else if (time < time_accelerating + cruise_time) {
-      // cruising
-      setpoint.velocity = max_vel;
-      setpoint.position = distance_while_accelerating + max_vel * (time - time_accelerating);
-    } else if (time < total_time) {
-      // slowing down
-      float time_decay = time - (time_accelerating + cruise_time);
-      setpoint.velocity = max_vel - time_decay * max_acc;
-      setpoint.position = distance_while_accelerating + cruise_distance
-        + (max_vel * time_decay)
-        - 0.5 * max_acc * time_decay * time_decay;
-    } else {
-      //done
-      setpoint.velocity = 0.0;
-      setpoint.position = dist;
-      setpoint.complete = true;
-    }
-  }
-
-  return setpoint;
-}
-
-float trapezoidal_time(struct Trapezoidal* trapezoidal) {
-  float max_vel = trapezoidal->max_vel;
-  float max_acc = trapezoidal->max_acc;
-  float dist = trapezoidal->dist;
-
-  float time_accelerating = max_acc / max_vel;
-  float distance_while_accelerating = 0.5 * max_acc * time_accelerating * time_accelerating;
-
-  if (2.0 * distance_while_accelerating > dist) {
-    // triangular
-    float peak_velocity_time = sqrt(dist/max_acc);
-    return peak_velocity_time * 2.0;
-  } else {
-    // trapezoidal
-    float cruise_distance = dist - 2.0 * distance_while_accelerating;
-    float cruise_time = cruise_distance / max_vel;
-    float total_time = 2 * time_accelerating + cruise_time;
-    return total_time;
-  }
-}