lib/LSM6/src/LSM6.cpp

@@ -31,13 +31,7 @@ bool LSM6::checkForNewData(void)
   bool retVal = false;
   if(getStatus() & 0x02)
   {
-    readGyro();
+    read();
-
-    retVal = true;
-  }
-  if(getStatus() & 0x01)
-  {
-    readAcc();
 
     retVal = true;
   }
@@ -238,7 +232,7 @@ void LSM6::enableDefault(void)
 
     // Set the accelerometer full scale and data rate
     setFullScaleAcc(ACC_FS2);
-    setAccDataOutputRate(ODR104);
+    setAccDataOutputRate(ODR13);
 
     // Auto-increment
     writeReg(CTRL3_C, 0x04);

lib/LineSensor/src/LineSensor.cpp

@@ -23,12 +23,7 @@ float LineSensor::AverageReflectance(void)
 }
 
 bool LineSensor::LineDetected() {
-#ifdef DARK
     return AverageReflectance() > LINE_THRESHOLD;
-#endif
-#ifndef DARK
-    return AverageReflectance() < 200;
-#endif
 }
 
 float LineSensor::CalcError(void) 
@@ -37,9 +32,6 @@ float LineSensor::CalcError(void)
     float sum = 0;
     for (int i = 0; i<sensorCount; i++) {
         int ret = analogRead(sensors[i]);
-#ifndef DARK
-        ret = 1000 - ret;
-#endif
         sum_pos += ret * (i+1);
         sum += ret;
 #ifdef __TRACK_DEBUG__
@@ -49,8 +41,6 @@ float LineSensor::CalcError(void)
     }
     float pos = sum_pos / sum;
 #ifdef __TRACK_DEBUG__
-    Serial.print(AverageReflectance());
-    Serial.print(" ");
     Serial.println(pos);
 #endif
     return pos;
@@ -59,10 +49,5 @@ float LineSensor::CalcError(void)
 
 bool LineSensor::CheckIntersection(void)
 {
-#ifdef DARK
     return AverageReflectance() > INTERSECTION_THRESHOLD;
-#endif
-#ifndef DARK
-    return AverageReflectance() < 100;
-#endif
 }

lib/LineSensor/src/LineSensor.h

@@ -2,17 +2,19 @@
 
 #include <Arduino.h>
 
+#define FIRST_LINE_SENSOR A0
+
 class LineSensor
 {
 protected:
     const static uint8_t sensorCount = 6;
-    const byte sensors[sensorCount] = {A0,A11,A2,A3,A4,A6};
+    const byte sensors[sensorCount] = {A0,A7,A2,A3,A4,A6};
 
     bool prevOnIntersection = false;
 
-public:
     float AverageReflectance();
 
+public:
     LineSensor(void) {}
     void Initialize(void);
     float CalcError(void); // varies between 1 and 6

platformio.ini

@@ -19,7 +19,6 @@ lib_deps =
     Wire
     https://github.com/gcl8a/IRDecoder    
     https://github.com/gcl8a/event_timer
-    https://github.com/WPIRoboticsEngineering/openmv-apriltags
 ;    https://github.com/gcl8a/LSM6
     https://github.com/WPIRoboticsEngineering/Romi-32u4-utilities
 

src/main.cpp

@@ -7,8 +7,6 @@ Robot robot;
 void setup() 
 {
   Serial.begin(250000);
-  delay(1000);
-  Serial1.begin(115200);
 
 #ifdef __LOOP_DEBUG__
   while(!Serial){delay(5);}

src/robot-cam.cpp

@@ -1,130 +0,0 @@
-#include "robot.h"
-#include <Wire.h>
-
-int blinkTagId = 0;
-int blinkTagIdInProgress = 0;
-int blinkWait = 1;
-int blinkWaitState = LOW;
-void HandleBlinkenLights(void) {
-    if (blinkTagIdInProgress == 0) {
-        blinkTagIdInProgress = blinkTagId;
-        blinkWait = 100;
-    }
-    if (blinkTagIdInProgress == 0) { return; }
-
-    if (blinkWait-- == 0) {
-        blinkWait = 25;
-
-        if (blinkWaitState == HIGH) {
-            Serial.print(blinkTagIdInProgress);
-            digitalWrite(13, blinkTagIdInProgress-- < 0);
-            blinkWaitState = LOW;
-        } else {
-            digitalWrite(13, HIGH);
-            blinkWaitState = HIGH;
-        }
-    }
-}
-
-int tagLostFrames = 30;
-uint8_t Robot::FindAprilTags(void) {
-  HandleBlinkenLights();
-  if(camera.checkUART(tag))
-  {
-    if (tag.id < 10) { // activate blinkenlights
-        tagId = tag.id;
-    }
-    Serial.print(F("Tag [cx="));
-    Serial.print(tag.cx);
-    Serial.print(F(", cy="));
-    Serial.print(tag.cy);
-    Serial.print(F(", w="));
-    Serial.print(tag.w);
-    Serial.print(F(", h="));
-    Serial.print(tag.h);
-    Serial.print(F(", id="));
-    Serial.print(tag.id);
-    Serial.print(F(", rot="));
-    Serial.print(tag.rot);
-    Serial.println(F("]"));
-    tagLostFrames = 30;
-    return 1;
-  }
-  
-  // stop every INT_MAX loops 30 loops after losing a tag
-  if (tagLostFrames--==0) {
-      if (robotState == ROBOT_APPROACHING) {
-          chassis.Stop();
-      }
-      blinkTagId = 0; // disable blinkenlights
-  }
-
-  return 0;
-}
-
-void Robot::HandleAprilTag(const AprilTagDatum &tag) {
-    // You may want to comment some of these out when you’re done testing.
-    //Serial.print("Tag: ");
-    //Serial.print(tag.id);
-    //Serial.print("\t");
-    //Serial.print(tag.cx);
-    //Serial.print("\t");
-    //Serial.print(tag.cy);
-    //Serial.print("\t");
-    //Serial.print(tag.h);
-    //Serial.print("\t");
-    //Serial.print(tag.w);
-    //Serial.print("\t");
-    //Serial.print(tag.rot); // rotated angle; try turning the tag
-    //Serial.print("\n");
-
-  if (robotState == ROBOT_SEARCHING) {
-    EnterApproachingState(); //found tag while searching
-  }
-  if (robotState == ROBOT_APPROACHING) {
-    if (CheckApproachComplete(headingTolerance, distanceTolerance)) {
-        HandleApproachComplete();
-        return;
-    }
-
-    float error = (float)tag.cx - (float)CENTERPOINT_X;
-
-    float effort = error*apriltag_Kp;
-
-    effort = max(effort, -1.5);
-    effort = min(effort,  1.5);
-
-    chassis.SetTwist(-baseSpeed, effort);
-  }
-}
-
-void Robot::EnterSearchingState(void) {
-    /** TODO: Set Romi to slowly spin to look for tags. */
-  Serial.println(" -> SEARCHING");
-  chassis.SetTwist(0.0,0.5);
-  robotState = ROBOT_SEARCHING;
-}
-
-void Robot::EnterApproachingState(void) {
-    /**
-     * TODO: Turn on the LED when the Romi finds a tag. For extra points,
-     * blink out a number that corresponds to the tag ID (must be non-blocking!).
-     * Be sure to add code (elsewhere) to turn the LED off when the Romi is
-     * done aligning.
-     */
-  Serial.println(" -> APPROACHING");
-  chassis.Stop();
-  robotState = ROBOT_APPROACHING;
-}
-
-/** Note that the tolerances are in integers, since the camera works
- * in integer pixels. If you have another method for calculations,
- * you may need floats.
- */
-bool Robot::CheckApproachComplete(int headingTolerance, int distanceTolerance) {
-    return ((tag.w > distanceTolerance) && abs(tag.cx - CENTERPOINT_X) < headingTolerance);
-}
-
-void Robot::HandleApproachComplete() {
-   EnterIdleState();
-}

src/robot-remote.cpp

@@ -43,11 +43,6 @@ void Robot::HandleKeyCode(int16_t keyCode)
     {
         switch(keyCode)
         {
-            case UP_ARROW:
-                baseSpeed = keyString.toInt();
-                EnterSearchingState();
-                keyString = "";
-                break;
             case REWIND:
                 EnterLineFollowing(keyString.toInt());
                 keyString = "";

src/robot.cpp

@@ -40,37 +40,28 @@ void Robot::EnterTurn(float angleInDeg)
 {
     Serial.println(" -> TURN");
     robotState = ROBOT_TURNING;
-    Serial.println(" -> TURN");
+
-    robotState = ROBOT_TURNING;
+    /**
-    targetDirection = (currDirection+2)%4;
+     * TODO: Add code to initiate the turn and set the target
-    chassis.SetTwist(0.0,0.5);
+     */
 }
 
 bool Robot::CheckTurnComplete(void)
 {
     bool retVal = false;
 
-    if (targetDirection == currDirection) {return retVal;};
+    /**
-
+     * TODO: add a checker to detect when the turn is complete
-    // unsound
+     */
-    if (lineSensor.LineDetected() && eulerAngles.z > targetDirection * 90) {
-        retVal = true;
-    }
 
     return retVal;
 }
 
 void Robot::HandleTurnComplete(void)
 {
-    currDirection = targetDirection;
+    /**
-    chassis.Stop();
+     * TODO: Add code to handle the completed turn
-
+     */
-    if (robotRampState == RAMP_ASCENDING) {
-        robotRampState = RAMP_DESCENDING;
-        Robot::EnterLineFollowing(20.);
-    } else {
-        Robot::EnterIdleState();
-    }
 }
 
 /**
@@ -90,29 +81,11 @@ void Robot::HandleOrientationUpdate(void)
     else // update orientation
     {
         float z_rot = ((float)imu.g.z - imu.gyroBias.z) / (float) INT_MAX;
-        z_rot /= 104.0;
+        z_rot /= 104;
-        z_rot *= 245.0;
+        z_rot *= 245;
         eulerAngles.z += z_rot;
-
     }
 
-    float g_lsb = 0.061 / 1000.;
-    float acc_x = asin(imu.a.x * g_lsb) * 180 / PI;
-    if (isnan(acc_x)) {
-            acc_x = 90; // pointing straight down will never occur
-    }
-    float gyrodelt_x = (float)imu.g.x / (float)INT_MAX;
-    gyrodelt_x *= 245.0 / 104.0;
-    float kappa = 0.1;
-    eulerAngles.x = (1-kappa) * (eulerAngles.x + gyrodelt_x) + kappa * acc_x;
-
-    float acc_y = asin(imu.a.y * g_lsb) * 180 / PI;
-    if (isnan(acc_y)) {
-            acc_y = 90; // pointing straight down will never occur
-    }
-    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');
@@ -140,18 +113,11 @@ int lineLostFrames = 24;
 
 float lastError = 0;
 
-int leveltimer = 0;
-
 void Robot::LineFollowingUpdate(void)
 {
     if(robotState == ROBOT_LINING) 
     {
-#ifdef DARK
         float setpoint = rollingTurnRate > 0.1 ? 4 : 3;
-#endif
-#ifndef DARK
-        float setpoint = 3.5;
-#endif
 
         float lineError = setpoint - lineSensor.CalcError();
 
@@ -209,32 +175,6 @@ void Robot::LineFollowingUpdate(void)
         }
 
         chassis.SetTwist(speed, turnEffort);
-
-        if (eulerAngles.x > 10.) {
-            Serial.println("climbing");
-        } else if (eulerAngles.x <= 10.) {
-            Serial.println("level");
-        }
-
-        if (eulerAngles.x > 10.) { // activate timer when on ramp
-            leveltimer = 30;
-        }
-        if (leveltimer > 0) {
-            if (eulerAngles.x <= 5.) {
-                leveltimer--; // count down when off ramp
-                if (leveltimer == 1) {
-                    leveltimer = 0;
-                    if (robotRampState == RAMP_ASCENDING) {
-                        Robot::EnterTurn(180);
-                    } else {
-                        EnterIdleState();
-                    }
-                }
-            } else {
-                leveltimer = 30;
-            }
-        }
-
     }
 }
 

src/robot.h

@@ -2,9 +2,7 @@
 #include "chassis.h"
 #include <LineSensor.h>
 #include <LSM6.h>
-#include <openmv.h>
 
-static float apriltag_Kp = 0.043; // rads per second per pixel
 static float lining_kP2 = 0.67;
 static float lining_kP = 0.72;
 static float lining_kD = -1.2;
@@ -24,13 +22,6 @@ protected:
     };
     ROBOT_CTRL_MODE robotCtrlMode = CTRL_TELEOP;
 
-    enum ROBOT_RAMP_STATE
-    {
-        RAMP_ASCENDING,
-        RAMP_DESCENDING,
-    };
-    ROBOT_RAMP_STATE robotRampState = RAMP_ASCENDING;
-
     /**
      * robotState is used to track the current task of the robot. You will add new states as 
      * the term progresses.
@@ -40,8 +31,6 @@ protected:
         ROBOT_IDLE, 
         ROBOT_LINING,
         ROBOT_TURNING,
-        ROBOT_SEARCHING,
-        ROBOT_APPROACHING,
     };
     ROBOT_STATE robotState = ROBOT_IDLE;
 
@@ -52,12 +41,6 @@ protected:
     LineSensor lineSensor;
 
     /* To add later: rangefinder, camera, etc.*/
-    uint16_t distanceTolerance = 65; //for width of tag
-    #define CENTER_THRESHOLD 10
-    #define CENTERPOINT_X 80
-    uint16_t headingTolerance = 10; //for heading cx of tag
-    AprilTagDatum tag;
-    OpenMV camera;
 
     // For managing key presses
     String keyString;
@@ -124,11 +107,4 @@ protected:
 
     /* For commanding the lifter servo */
     void SetLifter(uint16_t position);
-
-    uint8_t FindAprilTags(void);
-    void HandleAprilTag(const AprilTagDatum &tag);
-    void EnterSearchingState(void);
-    void EnterApproachingState(void);
-    bool CheckApproachComplete(int headingTolerance, int distanceTolerance);
-    void HandleApproachComplete();
 };