Checkpoint some shit that actually kinda works.

data_types.h

@@ -1,7 +1,7 @@
 #ifndef DataTypes_h
 #define DataTypes_h
 
-const int LOG_LEVEL_SILENT = 1;
+const int LOG_LEVEL_SILENT = 0;
 const int LOG_LEVEL_WARN = 1;
 const int LOG_LEVEL_INFO = 2;
 const int LOG_LEVEL_DEBUG = 3;

lisa-arduino.ino

@@ -6,14 +6,17 @@
 // *** DEFINE COURSE HERE ***
 const long EMERGENCY_TIMEOUT_MS = 1 * 60 * 1000;
 Waypoint waypoints[] = {
+  //Waypoint{10, 0, 0.3},
   // Left 10m square
+  /*
   Waypoint{8, 0, 0.3},
   Waypoint{10, 2, 0.3},
   Waypoint{10, 8, 0.3},
   Waypoint{8, 10, 0.3},
   Waypoint{2, 10, 0.3},
   Waypoint{0, 8, 0.3},
   Waypoint{0, -3, 3},
+  */
 
   // Right 10m square
   /*
@@ -27,7 +30,6 @@ Waypoint waypoints[] = {
   */
 
   // Figure 8
-  /*
   Waypoint{3, -3, 0.3},
   Waypoint{6, 0, 0.3},
   Waypoint{9, 3, 0.3},
@@ -37,7 +39,6 @@ Waypoint waypoints[] = {
   Waypoint{3, 3, 0.3},
   Waypoint{0, 0, 0.3},
   Waypoint{-4, 0, 3},
-  */
 };
 
 // PIN ASSIGNMENTS
@@ -49,14 +50,14 @@ const int WHEEL_ENCODER_INT = 0; // pin 2
 
 // TODO: Move into a class
 const long MOCK_WHEEL_ENCODER_PIN = 8;
-const bool MOCK_IMU = true;
+const bool MOCK_IMU = false;
 void handleMockSensors();
 
 // Create helper objects
-PositionTracker tracker(LOG_LEVEL_INFO);
-WaypointManager manager(LOG_LEVEL_INFO);
-Navigator navigator(LOG_LEVEL_DEBUG);
-int logLevel = LOG_LEVEL_INFO;
+PositionTracker tracker(LOG_LEVEL_SILENT);
+WaypointManager manager(LOG_LEVEL_SILENT);
+Navigator navigator(LOG_LEVEL_SILENT);
+int logLevel = LOG_LEVEL_SILENT;
 
 // Global values for wheel encoder and IMU
 volatile long gWheelEncoderTicks = 0;
@@ -140,16 +141,19 @@ void loop() {
 }
 
 // If IMU data available read it
-// TODO: Consider moving this to PositionTracker and just have it read and wait for data
 void serialEvent() {
   if (!MOCK_IMU) {
-    gRoll = Serial.parseFloat();
-    gPitch = Serial.parseFloat();
-    gYaw = Serial.parseFloat();
+    float roll = Serial.parseFloat();
+    float pitch = Serial.parseFloat();
+    float yaw = Serial.parseFloat();
     char garbage[20];
     Serial.readBytesUntil('\0', garbage, 20);
     //Serial.print("gYaw: ");
     //Serial.println(gYaw);
+    // NOTE: Sometimes the IMU spikes changes so limit the size we believe for a given fast loop rad
+    //if (abs(yaw - gYaw) < IMU_MAX_DELTA_DEGREES) {
+      gYaw = yaw;
+    //}
   }
 }
 

position_tracker.cpp

@@ -24,13 +24,6 @@ Position PositionTracker::update() {
   //Serial.print(gYaw);
   //Serial.print(", rDelta: ");
   //Serial.println(rDelta);
-  // NOTE: Sometimes the IMU spikes changes so limit the size we believe
-  if (abs(rDelta) > IMU_MAX_DELTA_RADIANS) {
-    // Skip this position update and get next time
-    // TODO: Return an error code so we can blink it out
-    return this->position;
-  }
-  // Successful read, store as last read
   this->lastYaw = gYaw;
 
   // Figure out wheel encoder delta, update stored value and calculate distance

position_tracker.h

@@ -10,7 +10,7 @@ extern volatile float gPitch;
 extern volatile float gRoll;
 extern volatile float gYaw;
 
-const float IMU_MAX_DELTA_RADIANS = 10.0 * PI / 180.0;
+const float IMU_MAX_DELTA_DEGREES = 10.0;
 
 // TODO: Put the actual things measured in here and use to compute M so that it's documented
 const float WHEEL_ENCODER_M_DISTANCE_FROM_TICKS = 0.0544737;

speed_calibration.h

@@ -9,7 +9,7 @@ const float SPEED_B_SERVO_FROM_VELOCITY = 659.19;
 
 const int SPEED_STOPPED_SERVO = 0;
 // NOTE: We can directly set velocity and servo instead of curve evaluation if we like
-const float SPEED_LOW_VELOCITY = 0.5;
+const float SPEED_LOW_VELOCITY = 1.0;
 const int SPEED_LOW_SERVO = max(min(int(SPEED_M_SERVO_FROM_VELOCITY * SPEED_LOW_VELOCITY + SPEED_B_SERVO_FROM_VELOCITY), SPEED_MAX_SERVO), SPEED_MIN_SERVO);
 // NOTE: If you bump up the top speed you should also make it do updates in the main loop faster or allow more IMU angle variation, otherwise we turn to fast to track
 const float SPEED_HIGH_VELOCITY = 2.0;

steering_calibration.h

@@ -14,18 +14,18 @@ const float INCH_TO_M = 0.0254;
 // and pick to either turn 0, fixed left or fixed right, where we have the
 // direct correlation to left and right determined already.
 const float STEERING_CENTER_SERVO = 1608; // Not (STEERING_MAX_SERVO + STEERING_MIN_SERVO) / 2 by experimentation
-//const float STEERING_LEFT_SERVO = 1864; // half to max
-//const float STEERING_LEFT_TURN_RADIUS_INCH = 146;
-//const float STEERING_LEFT_TURN_RADIUS = STEERING_LEFT_TURN_RADIUS_INCH * INCH_TO_M; // m
-const float STEERING_LEFT_SERVO = 2120; // max
-const float STEERING_LEFT_TURN_RADIUS_INCH = 77;
+const float STEERING_LEFT_SERVO = 1864; // half to max
+const float STEERING_LEFT_TURN_RADIUS_INCH = 146;
 const float STEERING_LEFT_TURN_RADIUS = STEERING_LEFT_TURN_RADIUS_INCH * INCH_TO_M; // m
-//const float STEERING_RIGHT_SERVO = 1379; // picked half way to max
-//const float STEERING_RIGHT_TURN_RADIUS_INCH = -116;
-//const float STEERING_RIGHT_TURN_RADIUS = STEERING_RIGHT_TURN_RADIUS_INCH * INCH_TO_M; // m
-const float STEERING_RIGHT_SERVO = 1150; // picked half way to max
-const float STEERING_RIGHT_TURN_RADIUS_INCH = -66;
+//const float STEERING_LEFT_SERVO = 2120; // max
+//const float STEERING_LEFT_TURN_RADIUS_INCH = 77;
+//const float STEERING_LEFT_TURN_RADIUS = STEERING_LEFT_TURN_RADIUS_INCH * INCH_TO_M; // m
+const float STEERING_RIGHT_SERVO = 1379; // picked half way to max
+const float STEERING_RIGHT_TURN_RADIUS_INCH = -116;
 const float STEERING_RIGHT_TURN_RADIUS = STEERING_RIGHT_TURN_RADIUS_INCH * INCH_TO_M; // m
+//const float STEERING_RIGHT_SERVO = 1150; // picked half way to max
+//const float STEERING_RIGHT_TURN_RADIUS_INCH = -66;
+//const float STEERING_RIGHT_TURN_RADIUS = STEERING_RIGHT_TURN_RADIUS_INCH * INCH_TO_M; // m
 
 //// Pick our 30 and 10 degree turn radiuses based on what we can hit with both left and right to standardize
 //const float TURN_RADIUS_FOR_30_DEGREES = 1.0; // m