mended units

robot_controller/constants.h

@@ -0,0 +1,15 @@
+#pragma once
+
+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
+// centimeters per second to rpm
+const float CMS_RPM = 60.0 / (PI*WHEEL_DIAMETER);
+
+const float TURN_AMOUNT = 180.0; // degrees
+const float RETURN_DISTANCE = 100.0; // cm

robot_controller/robot_controller.ino

@@ -3,17 +3,7 @@
 #include <smartmotor.h>
 #include <SMC_gains.h>
 #include "trapezoidal.h"
-
+#include "constants.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); 
@@ -63,7 +53,7 @@ void setup() {
 }
 
 void loop() {
-  float time = (float)millis() / 1000.0;
+  float time = (float)millis() / 1000.0 - phase_start;
   switch (robot_state) {
     case FORWARD:
       setpoint = trapezoidal_planner(&forward, time);
@@ -76,7 +66,7 @@ void loop() {
 
 
 
-  left_motor.write_rpm(velocity);
+  left_motor.write_rpm(setpoint.velocity * CMS_RPM);
   //right_motor.write_rpm(robot_state == TURN ? velocity : -velocity);
   delay(20);
 

robot_controller/trapezoidal.ino

@@ -8,7 +8,7 @@ struct Setpoint trapezoidal_planner(struct Trapezoidal* trapezoidal, float time)
 
   struct Setpoint setpoint = {0.0, 0.0};
 
-  float time_accelerating = max_acc / max_vel;
+  float time_accelerating = max_vel / max_acc;
   float distance_while_accelerating = 0.5 * max_acc * time_accelerating * time_accelerating;
 
   if (2.0 * distance_while_accelerating > dist) {
@@ -48,7 +48,7 @@ struct Setpoint trapezoidal_planner(struct Trapezoidal* trapezoidal, float time)
     } else if (time < total_time) {
       // slowing down
       float time_decay = time - (time_accelerating + cruise_time);
-      setpoint.velocity = max_vel - time_decay * max_acc;
+      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;