apriltag tracking

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.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;
@@ -38,6 +40,8 @@ protected:
         ROBOT_IDLE, 
         ROBOT_LINING,
         ROBOT_TURNING,
+        ROBOT_SEARCHING,
+        ROBOT_APPROACHING,
     };
     ROBOT_STATE robotState = ROBOT_IDLE;
 
@@ -48,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;
@@ -114,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();
 };