diff

lib/Chassis/src/chassis.cpp

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

platformio.ini

@@ -22,8 +22,11 @@ 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
-        tagId = tag.id;
+        blinkTagId = 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,6 +95,29 @@ 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;
         }
     }
 
@@ -109,6 +132,7 @@ 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;
@@ -121,6 +145,7 @@ 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;
@@ -179,4 +204,4 @@ void Robot::EnterSetupMode(void)
 {
     Serial.println("-> SETUP");
     robotCtrlMode = CTRL_SETUP;
-}
+}

src/robot.cpp

@@ -23,6 +23,10 @@ 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)
@@ -81,6 +85,8 @@ 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
@@ -113,16 +119,42 @@ 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(imu.gyroBias.z);
-    Serial.print('\t');
-    Serial.print(eulerAngles.z);
-    Serial.print('\t');
+    //Serial.print(millis());
+    //Serial.print('\t');
+    //Serial.print(imu.gyroBias.z);
+    //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
 }
 
@@ -153,8 +185,8 @@ void Robot::LineFollowingUpdate(void)
         float setpoint = 3.5;
 #endif
 
-        float lineError = setpoint - lineSensor.CalcError();
 
+        float lineError = setpoint - lineSensor.CalcError();
 
         float powErr = (lineError*lineError);
         powErr = (lineError < 0 ? -powErr : powErr);
@@ -244,55 +276,14 @@ void Robot::LineFollowingUpdate(void)
  */
 void Robot::HandleIntersection(void)
 {
-    Serial.print("X: ");
-    if(robotState == ROBOT_LINING) 
-    {
-        switch(nodeTo)
-        {
-            case NODE_START:
-                if(nodeFrom == NODE_1)
-                    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');
+    //Serial.print("Y: ");
+    //Serial.println(pj);
+    if(robotState == ROBOT_LINING) {
+        // update location
+        if (currDirection == EAST) pi++;
+        if (currDirection == WEST) pi--;
+        if (currDirection == NORTH) pj++;
+        if (currDirection == SOUTH) pj--;
     }
 }
 
@@ -303,6 +294,8 @@ void Robot::RobotLoop(void)
      * and asynchronous events (IR presses, distance readings, etc.).
     */
 
+    servo.update();
+
     /**
      * Handle any IR remote keypresses.
      */
@@ -314,6 +307,7 @@ void Robot::RobotLoop(void)
      */
     if(chassis.CheckChassisTimer())
     {
+        //Serial.println(lineSensor.CalcError());
         // add synchronous, pre-motor-update actions here
         if(robotState == ROBOT_LINING)
         {
@@ -326,6 +320,8 @@ void Robot::RobotLoop(void)
 
     }
 
+    lineSensor.CalcError();
+
     /**
      * Check for any intersections
      */
@@ -339,5 +335,15 @@ 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,6 +3,8 @@
 #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;
@@ -51,6 +53,10 @@ 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
@@ -80,14 +86,15 @@ protected:
     float turnRateDamp = 0.975; // the proportion of the turn rate retained each loop
     float KTurnRate = 0.18; // slowdown coefficient
 
-    /**
-     * For tracking the motion of the Romi. We keep track of the intersection we came
-     * from and the one we're headed to. You'll program in the map in handleIntersection()
-     * and other functions.
-     */
-    enum INTERSECTION {NODE_START, NODE_1, NODE_2, NODE_3,};
-    INTERSECTION nodeFrom = NODE_START;
-    INTERSECTION nodeTo = NODE_1;
+    uint8_t pi=0, pj=0;
+    int8_t currDirection = NORTH;
+    int8_t targetDirection = NORTH;
+    enum Direction {
+        NORTH = 1,
+        EAST = 0,
+        SOUTH = 3,
+        WEST = 2,
+    };
     
 public:
     Robot(void) {keyString.reserve(8);} //reserve some memory to avoid reallocation