lib/Chassis/src/chassis.cpp

@@ -155,11 +155,4 @@ void Chassis::SetTwist(float fwdSpeed, float rotSpeed)
     float rightSpeed = fwdSpeed + rotSpeed * ROBOT_RADIUS;
 
     SetWheelSpeeds(leftSpeed, rightSpeed);
-}
+}
-
-// cm/s
-float Chassis::GetAverageSpeed() {
-    float average_ticks_per_loop = (leftMotor.speed + rightMotor.speed)/2;
-    float average_cm_per_s = average_ticks_per_loop / LEFT_TICKS_PER_CM / CONTROL_LOOP_PERIOD_S;
-    return average_cm_per_s;
-}

lib/Chassis/src/chassis.h

@@ -39,7 +39,6 @@ public:
     void SetTwist(float fwdSpeed, float rotSpeed);
     void SetWheelSpeeds(float, float);
     void UpdateMotors(void);
-    float GetAverageSpeed();
 
 protected:
     /**

lib/LSM6/src/LSM6.cpp

@@ -358,4 +358,4 @@ int16_t LSM6::testReg(uint8_t address, regAddr reg)
   {
     return TEST_REG_ERROR;
   }
-}
+}

lib/Romi32U4Motors/src/Romi32U4MotorTemplate.h

@@ -133,8 +133,6 @@ protected:
             currentSetpoint += deltaSetpoint;
 
             float error = currentSetpoint - speed;
-            //Serial.println(speed);
-            //Serial.println(targetSpeed);
 
             sumError += error; 
 
@@ -167,13 +165,13 @@ protected:
 
             if(debug)
             {            
-                Serial.print("");
+                Serial.print("target:");
                 Serial.print(targetSpeed);
-                Serial.print("\t");
+                Serial.print("\tspeed:");
                 Serial.print(speed);
-                Serial.print("\t");
+                Serial.print("\terror:");
                 Serial.print(error);
-                Serial.print("\t");
+                Serial.print("\teffort:");
                 Serial.print(effort / 10.0); // N.B. that we divide by 10 to make the graph cleaner
                 Serial.print('\n');
             }

platformio.ini

@@ -22,11 +22,8 @@ lib_deps =
     https://github.com/WPIRoboticsEngineering/openmv-apriltags
 ;    https://github.com/gcl8a/LSM6
     https://github.com/WPIRoboticsEngineering/Romi-32u4-utilities
-    https://github.com/gcl8a/Load-Cell-Amps
 
 build_flags = 
 ;    -D__MOTOR_DEBUG__
 ;    -D__LOOP_DEBUG__
-;    -D__IMU_DEBUG__
+;    -D__IMU_DEBUG__
-;    -D__TRACK_DEBUG__
-;    -DDARK

src/robot-cam.cpp

@@ -32,7 +32,7 @@ uint8_t Robot::FindAprilTags(void) {
   if(camera.checkUART(tag))
   {
     if (tag.id < 10) { // activate blinkenlights
-        blinkTagId = tag.id;
+        tagId = tag.id;
     }
     Serial.print(F("Tag [cx="));
     Serial.print(tag.cx);

src/robot-remote.cpp

@@ -17,7 +17,7 @@ IRDecoder decoder(IR_PIN);
 
 void Robot::HandleKeyCode(int16_t keyCode)
 { 
-    //Serial.println(keyCode);
+    Serial.println(keyCode);
 
     // Regardless of current state, if ENTER is pressed, go to idle state
     if(keyCode == STOP_MODE) EnterIdleState();
@@ -95,29 +95,6 @@ void Robot::HandleKeyCode(int16_t keyCode)
             case ENTER_SAVE:
                 chassis.SetTwist(0, 0);
                 break;
-            case REWIND:
-                servo.setTargetPos(keyString.toInt());
-                Serial.println(keyString);
-                keyString = "";
-                break;
-            case NUM_1:
-            case NUM_2:
-            case NUM_3:
-                keyString += (char)(keyCode + 33);
-                break;
-            case NUM_4:
-            case NUM_5:
-            case NUM_6:
-                keyString += (char)(keyCode + 32);
-                break;
-            case NUM_7:
-            case NUM_8:
-            case NUM_9:
-                keyString += (char)(keyCode + 31);
-                break;
-            case NUM_0_10:
-                keyString += '0';
-                break;
         }
     }
 
@@ -132,7 +109,6 @@ void Robot::HandleKeyCode(int16_t keyCode)
             case VOLminus:
                 k = keyString.toInt() / 100.0;
                 Serial.print("Kp = "); Serial.println(k);
-                //lining_kP = k;
                 chassis.SetMotorKp(k);
                 keyString = "";
                 break;
@@ -145,7 +121,6 @@ void Robot::HandleKeyCode(int16_t keyCode)
             case VOLplus:
                 k = keyString.toInt() / 100.0;
                 Serial.print("Kd = "); Serial.println(k);
-                //lining_kD = k;
                 chassis.SetMotorKd(k);
                 keyString = "";
                 break;
@@ -204,4 +179,4 @@ void Robot::EnterSetupMode(void)
 {
     Serial.println("-> SETUP");
     robotCtrlMode = CTRL_SETUP;
-}
+}

src/robot.cpp

@@ -23,10 +23,6 @@ void Robot::InitializeRobot(void)
 
     // The line sensor elements default to INPUTs, but we'll initialize anyways, for completeness
     lineSensor.Initialize();
-
-    servo.attach();  
-
-    loadCellHX1.Init();
 }
 
 void Robot::EnterIdleState(void)
@@ -85,8 +81,6 @@ void Robot::HandleTurnComplete(void)
 void Robot::HandleOrientationUpdate(void)
 {
     prevEulerAngles = eulerAngles;
-
-
     if(robotState == ROBOT_IDLE)
     {
         // TODO: You'll need to add code to LSM6 to update the bias
@@ -119,42 +113,16 @@ void Robot::HandleOrientationUpdate(void)
     float gyrodelt_y = (float)imu.g.y / (float)INT_MAX;
     gyrodelt_y *= 245.0 / 104.0;
     eulerAngles.y = (1-kappa) * (eulerAngles.y + gyrodelt_y) + kappa * acc_y;
-
 #ifdef __IMU_DEBUG__
-    //Serial.print(millis());
+    Serial.print(imu.gyroBias.z);
-    //Serial.print('\t');
+    Serial.print('\t');
-    //Serial.print(imu.gyroBias.z);
+    Serial.print(eulerAngles.z);
-    //Serial.print('\t');
+    Serial.print('\t');
-    //Serial.print(eulerAngles.z);
-    //Serial.print('\t');
     //Serial.print(imu.g.x);
     //Serial.print('\t');
     //Serial.print(imu.g.y);
     //Serial.print('\t');
-    //Serial.println(imu.g.z);
+    Serial.println(imu.g.z);
-    
-    //float g_lsb = 0.061 / 1000.;
-    //Serial.print(imu.a.x * g_lsb);
-    //Serial.print('\t');
-    //Serial.print(asin(imu.a.x * g_lsb) * 180 / PI);
-    //Serial.print('\t');
-    //Serial.println(eulerAngles.x);
-    //Serial.print('\t');
-    //Serial.print(imu.a.y * g_lsb);
-    //Serial.print('\t');
-    //Serial.println(imu.a.z * g_lsb);
-    
-    Serial.print(eulerAngles.x + gyrodelt_x);
-    Serial.print('\t');
-    Serial.print(acc_x);
-    Serial.print('\t');
-    Serial.println(eulerAngles.x);
-
-    //Serial.print(eulerAngles.x);
-    //Serial.print('\t');
-    //Serial.print(eulerAngles.y);
-    //Serial.print('\t');
-    //Serial.println(eulerAngles.z);
 #endif
 }
 
@@ -185,9 +153,9 @@ void Robot::LineFollowingUpdate(void)
         float setpoint = 3.5;
 #endif
 
-
         float lineError = setpoint - lineSensor.CalcError();
 
+
         float powErr = (lineError*lineError);
         powErr = (lineError < 0 ? -powErr : powErr);
 
@@ -276,14 +244,55 @@ void Robot::LineFollowingUpdate(void)
  */
 void Robot::HandleIntersection(void)
 {
-    //Serial.print("Y: ");
+    Serial.print("X: ");
-    //Serial.println(pj);
+    if(robotState == ROBOT_LINING) 
-    if(robotState == ROBOT_LINING) {
+    {
-        // update location
+        switch(nodeTo)
-        if (currDirection == EAST) pi++;
+        {
-        if (currDirection == WEST) pi--;
+            case NODE_START:
-        if (currDirection == NORTH) pj++;
+                if(nodeFrom == NODE_1)
-        if (currDirection == SOUTH) pj--;
+                    EnterIdleState();
+                break;
+            case NODE_1:
+                // By default, we'll continue on straight
+                if(nodeFrom == NODE_START) 
+                {
+                    nodeTo = NODE_2;
+                }
+                else if(nodeFrom == NODE_2)
+                {
+                    nodeTo = NODE_START;
+                }
+                nodeFrom = NODE_1;
+                break;
+            case NODE_2:
+                // By default, we'll continue on straight
+                if(nodeFrom == NODE_1) 
+                {
+                    nodeTo = NODE_3;
+                }
+                else if(nodeFrom == NODE_3)
+                {
+                    nodeTo = NODE_1;
+                }
+                nodeFrom = NODE_2;
+                break;
+            case NODE_3:
+                // By default, we'll bang a u-ey
+                if(nodeFrom == NODE_2) 
+                {
+                    nodeTo = NODE_2;
+                    nodeFrom = NODE_3;
+                    EnterTurn(180);
+                }
+                break;
+            default:
+                break;
+        }
+        Serial.print(nodeFrom);
+        Serial.print("->");
+        Serial.print(nodeTo);
+        Serial.print('\n');
     }
 }
 
@@ -294,8 +303,6 @@ void Robot::RobotLoop(void)
      * and asynchronous events (IR presses, distance readings, etc.).
     */
 
-    servo.update();
-
     /**
      * Handle any IR remote keypresses.
      */
@@ -307,7 +314,6 @@ void Robot::RobotLoop(void)
      */
     if(chassis.CheckChassisTimer())
     {
-        //Serial.println(lineSensor.CalcError());
         // add synchronous, pre-motor-update actions here
         if(robotState == ROBOT_LINING)
         {
@@ -320,8 +326,6 @@ void Robot::RobotLoop(void)
 
     }
 
-    lineSensor.CalcError();
-
     /**
      * Check for any intersections
      */
@@ -335,15 +339,5 @@ void Robot::RobotLoop(void)
     {
         HandleOrientationUpdate();
     }
-
-    if (FindAprilTags()) HandleAprilTag(tag);
-
-    int32_t reading = 0;
-    if(loadCellHX1.GetReading(reading)) 
-    {
-    //Serial.print(millis());
-    //Serial.print('\t');
-    Serial.println(reading);
-    }
 }
 

src/robot.h

@@ -3,8 +3,6 @@
 #include <LineSensor.h>
 #include <LSM6.h>
 #include <openmv.h>
-#include <servo32u4.h>
-#include <HX711.h>
 
 static float apriltag_Kp = 0.043; // rads per second per pixel
 static float lining_kP2 = 0.67;
@@ -53,10 +51,6 @@ protected:
     /* Line sensor */
     LineSensor lineSensor;
 
-    Servo32U4Pin5 servo;
-
-    HX711<6, 13> loadCellHX1;
-
     /* To add later: rangefinder, camera, etc.*/
     uint16_t distanceTolerance = 65; //for width of tag
     #define CENTER_THRESHOLD 10
@@ -86,15 +80,14 @@ protected:
     float turnRateDamp = 0.975; // the proportion of the turn rate retained each loop
     float KTurnRate = 0.18; // slowdown coefficient
 
-    uint8_t pi=0, pj=0;
+    /**
-    int8_t currDirection = NORTH;
+     * For tracking the motion of the Romi. We keep track of the intersection we came
-    int8_t targetDirection = NORTH;
+     * from and the one we're headed to. You'll program in the map in handleIntersection()
-    enum Direction {
+     * and other functions.
-        NORTH = 1,
+     */
-        EAST = 0,
+    enum INTERSECTION {NODE_START, NODE_1, NODE_2, NODE_3,};
-        SOUTH = 3,
+    INTERSECTION nodeFrom = NODE_START;
-        WEST = 2,
+    INTERSECTION nodeTo = NODE_1;
-    };
     
 public:
     Robot(void) {keyString.reserve(8);} //reserve some memory to avoid reallocation