Diff: Tracker.cpp

Revision:

0:8038ea3ee241

Child:

1:008116b6eb46

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Tracker.cpp	Thu Jan 26 20:06:55 2017 +0000
@@ -0,0 +1,99 @@
+#include "Tracker.h"
+
+Tracker::Tracker(PinName _chA, PinName _chB, PinName _pot) : chA(_chA), chB(_chB), pot(_pot)
+{
+    //chA = p29, chB = p30, pot = p15
+    wheel = new QEI(_chA, _chB, NC, 128, QEI::X4_ENCODING);   //orange, yellow wires, VU 5 VOLTS, BOTTOM 2x2 pins on WW02
+    encoding = 2;                                       // X2 encoding
+    circumference = 13.25;                              // circumference of wheel
+    constant = 1360;                                    // obtained from tests
+    dir = 3;                                            // starting dir
+    df = 4.50;                                          // distance from pivot to front wheel
+    db = 4.25;                                          // distance from pivot to back wheel
+}
+
+/*Tracker::~Tracker()
+{
+    if(wheel != NULL) {
+        delete wheel;
+    }   
+}*/
+
+void Tracker::clear() {
+    distance = 0;
+    x = 0;
+    y = 0;
+}
+
+void Tracker::potSetup() {
+    zeroAngle = (float) pot;
+    // USE A BETTER CALIBRATION PERIOD
+    for(int i = 5; i > 0; i--) {
+        printf("%d seconds to set turning angle.\n\r", i);
+        wait(1);
+    }
+    turnAngle = ((float) pot - zeroAngle) * 300;
+}
+
+void Tracker::setDirection() {
+    if (turnAngle > 0.5) {          // +/- 0.5 leeway
+        printf("DIRECTION SET TO CCW with turnAngle %f\n\r", turnAngle);
+        dir = CCW;
+        printf("%d is direction\n\r", dir);
+    } else if (turnAngle < -0.5) {
+        printf("DIRECTION SET TO CW with turnAngle %f\n\r", turnAngle);
+        dir = CW;
+    } else {
+        printf("DIRECTION SET TO STRAIGHT with turnAngle %f\n\r", turnAngle);
+        dir = STRAIGHT;
+    }
+    turnAngle = abs(turnAngle);
+}
+
+void Tracker::checkToStart() {               // start tracking when distance > 0
+    while(true) {
+        pulseCount = (float) wheel->getPulses();
+        distance = pulseCount*circumference/encoding/constant;
+        if (distance > 0) {
+            return;
+        } else {
+            wait(0.0001);
+        }
+    }
+}
+
+void Tracker::calcDisplacement() {
+    pulseCount = (float) wheel->getPulses();
+    distance = pulseCount*circumference/encoding/constant;
+
+    W = sqrt(df*df+db*db-2*df*db*cos(PI-turnAngle*PI/180));
+    R = W/(2*sin(0.5*turnAngle*PI/180));
+    dispAngle = distance/R;
+
+    if (dir == STRAIGHT) {
+        x = 0;
+        y = distance;
+    } else if (dir == CCW) {
+        x = -1*(R*(1-cos(dispAngle)));
+        y = R*sin(dispAngle);
+    } else if (dir == CW) {
+        x = R*(1-cos(dispAngle));
+        y = R*sin(dispAngle);
+    } else {
+        printf("Direction not set. TERMINATED. \n\r");
+        while(true) {}
+    }
+
+    wait(.0001);
+    printf("----------------------\n\r");
+    printf("x: %f\n\r", x);
+    printf("y: %f\n\r", y);
+    printf("distance: %f\n\r", distance);
+    printf("turnAngle: %f\n\r", turnAngle);
+    printf("dispAngle: %f\n\r", dispAngle);
+    printf("direction: %d\n\r", dir);
+    printf("W: %f\n\r", W);
+    printf("R: %f\n\r", R);
+}
+
+