apriltag tracking

lib/LSM6/src/LSM6.cpp

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

lib/LineSensor/src/LineSensor.cpp

@@ -23,7 +23,12 @@ float LineSensor::AverageReflectance(void)
 }
 
 bool LineSensor::LineDetected() {
+#ifdef DARK
     return AverageReflectance() > LINE_THRESHOLD;
+#endif
+#ifndef DARK
+    return AverageReflectance() < 200;
+#endif
 }
 
 float LineSensor::CalcError(void) 
@@ -32,6 +37,9 @@ 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__
@@ -41,6 +49,8 @@ float LineSensor::CalcError(void)
     }
     float pos = sum_pos / sum;
 #ifdef __TRACK_DEBUG__
+    Serial.print(AverageReflectance());
+    Serial.print(" ");
     Serial.println(pos);
 #endif
     return pos;
@@ -49,5 +59,10 @@ 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,19 +2,17 @@
 
 #include <Arduino.h>
 
-#define FIRST_LINE_SENSOR A0
-
 class LineSensor
 {
 protected:
     const static uint8_t sensorCount = 6;
-    const byte sensors[sensorCount] = {A0,A7,A2,A3,A4,A6};
+    const byte sensors[sensorCount] = {A0,A11,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,6 +19,7 @@ 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,6 +7,8 @@ Robot robot;
 void setup() 
 {
   Serial.begin(250000);
+  delay(1000);
+  Serial1.begin(115200);
 
 #ifdef __LOOP_DEBUG__
   while(!Serial){delay(5);}

src/robot-cam.cpp

@@ -0,0 +1,130 @@
+#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,6 +43,11 @@ 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,28 +40,37 @@ void Robot::EnterTurn(float angleInDeg)
 {
     Serial.println(" -> TURN");
     robotState = ROBOT_TURNING;
-
-    /**
-     * TODO: Add code to initiate the turn and set the target
-     */
+    Serial.println(" -> TURN");
+    robotState = ROBOT_TURNING;
+    targetDirection = (currDirection+2)%4;
+    chassis.SetTwist(0.0,0.5);
 }
 
 bool Robot::CheckTurnComplete(void)
 {
     bool retVal = false;
 
-    /**
-     * TODO: add a checker to detect when the turn is complete
-     */
+    if (targetDirection == currDirection) {return retVal;};
+
+    // unsound
+    if (lineSensor.LineDetected() && eulerAngles.z > targetDirection * 90) {
+        retVal = true;
+    }
 
     return retVal;
 }
 
 void Robot::HandleTurnComplete(void)
 {
-    /**
-     * TODO: Add code to handle the completed turn
-     */
+    currDirection = targetDirection;
+    chassis.Stop();
+
+    if (robotRampState == RAMP_ASCENDING) {
+        robotRampState = RAMP_DESCENDING;
+        Robot::EnterLineFollowing(20.);
+    } else {
+        Robot::EnterIdleState();
+    }
 }
 
 /**
@@ -81,11 +90,29 @@ void Robot::HandleOrientationUpdate(void)
     else // update orientation
     {
         float z_rot = ((float)imu.g.z - imu.gyroBias.z) / (float) INT_MAX;
-        z_rot /= 104;
-        z_rot *= 245;
+        z_rot /= 104.0;
+        z_rot *= 245.0;
         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');
@@ -113,11 +140,18 @@ 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();
 
@@ -175,6 +209,32 @@ 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,7 +2,9 @@
 #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;
@@ -22,6 +24,13 @@ 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.
@@ -31,6 +40,8 @@ protected:
         ROBOT_IDLE, 
         ROBOT_LINING,
         ROBOT_TURNING,
+        ROBOT_SEARCHING,
+        ROBOT_APPROACHING,
     };
     ROBOT_STATE robotState = ROBOT_IDLE;
 
@@ -41,6 +52,12 @@ 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;
@@ -107,4 +124,11 @@ 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();
 };