Revision 1:17db90f61f6c, committed 2016-11-29

Comitter:

simplyellow

Date:

Tue Nov 29 21:16:58 2016 +0000

Parent:

0:d4bb648b7f2c

Child:

2:e68fd2af041d

Commit message:

final 2d tracking

Changed in this revision

--- a/main.cpp	Mon Nov 14 21:46:17 2016 +0000
+++ b/main.cpp	Tue Nov 29 21:16:58 2016 +0000
@@ -2,209 +2,206 @@
 #include "mbed.h"
 #include "Motor.h"
 
-/*
-    Written by Milo Pan with the help of Jacob Callahan
-*/
-
-/*
-Use X4 encoding.
-QEI wheel(p29, p30, NC, 624, QEI::X4_ENCODING);
-QEI constructor(CHANNEL A, CHANNEL B,PinName index (NC), int pulsesPerRev, Encoding encoding=X2_ENCODING) 
-Use X2 encoding by default.
-Encoding x4 = x4_ENCODING;
-*/
-
-/*
-Measurement Instructions
-in order to convert the numerical value (outputted by this program) to Inches traveled,
-measurements need to be taken (i.e. have the truck travel 40" for 10 trials, average the values, and find the factor
-*/
-
 Serial pc(USBTX, USBRX);
 
-QEI wheel (p29, p30, NC, 128, QEI::X4_ENCODING);    //orange, yellow wires, VU 5 VOLTS, BOTTOM 2x2 pins on WW02
-float pulseCount;                                   //number of pulses detected by encoder
-float encoding = 2;                                 //X2
-float circumference = 13.25;                        //inches of circumference
+QEI wheel (p29, p30, NC, 128, QEI::X4_ENCODING);
+//orange, yellow wires, VU 5 VOLTS, BOTTOM 2x2 pins on WW02
+
+float pulseCount;
+float encoding = 2;
+float circumference = 13.25;
+float constant = 1360;              // constant = 20*68
+
 float distance;
 
+float x;
+float y;
+
 AnalogIn pot(p15);
 float value;
-float angle;
-float zero;
+float turnAngle;
+float zeroAngle;
+
+float PI = 3.14159265;
+
+#define STRAIGHT 0
+#define CCW 1
+#define CW 2
+
+int dir = 3;
+
+float df = 4.50;                    // distance from pivot to front wheel
+float db = 4.25;                    // distance from pivot to back wheel
+float W;
+float R;
+float dispAngle;
+
+#define Q1 1
+#define Q2 2
+#define Q3 3
+#define Q4 4
+
+int quadrant = 0;
 
-float PI = 3.14159;
+void clear() {
+    distance = 0;
+    x = 0;
+    y = 0;
+}
+
+void potSetup() {
+    zeroAngle = (float) pot;
+    for(int i = 5; i > 0; i--) {
+        pc.printf("%d seconds to set turning angle.\n\r", i);
+        wait(1);
+    }
+    turnAngle = ((float) pot - zeroAngle) * 300;
+    pc.printf("---------------------------------\n\r");
+    //debug
+    /*
+    for(int i = 5; i > 0; i--) {
+        pc.printf("%f is the turning Angle.\n\r", turnAngle);
+        pc.printf("%f is the zero value.\n\r", zeroAngle);
+        pc.printf("%f is the pot value.\n\r", (float) pot);
+        wait(1);
+    }*/
+}
 
-/* VERSION 1
-float x = 0;
-float dx;
-float y = 0;
-float dy;
-float dispAngle;        // theta d
-float r;                // r
-float c;                // c, chord length
+void setDirection() {
+    if (turnAngle > 0.5) {          // +/- 0.5 leeway
+        pc.printf("DIRECTION SET TO CCW with turnAngle %f\n\r", turnAngle);
+        dir = CCW;
+        pc.printf("%d is direction\n\r", dir);
+    } else if (turnAngle < -0.5) {
+        pc.printf("DIRECTION SET TO CW with turnAngle %f\n\r", turnAngle);
+        dir = CW;
+    } else {
+        pc.printf("DIRECTION SET TO STRAIGHT with turnAngle %f\n\r", turnAngle);
+        dir = STRAIGHT;
+    }
+    turnAngle = abs(turnAngle);
+}
 
-float segment;          // intermediary variable for ease of calculation
+void setQuadrant() {
+    if ((turnAngle > 0) && (turnAngle < 90)) {
+        quadrant = Q1;
+    } else if ((turnAngle > 90) && (turnAngle < 180)) {
+        quadrant = Q2;
+    } else if ((turnAngle > 180) && (turnAngle < 270)) {
+        quadrant = Q3;
+    } else if ((turnAngle > 270) && (turnAngle < 360)) {
+        quadrant = Q4;
+    } else if (turnAngle == 0) {
+        //don't worry
+    } else {
+        pc.printf("turnAngle not valid. TERMINATED. \n\r");
+        while(true) {}
+    }
+}
 
-float heading;          // heading of front
+void 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);
+        }
+    }
+}
+/*
+AXES
+when robot is facing forward
+forward is +x
+left is +y
 */
 
 
-// VERSION 2
-float df;
-float db;
-float circumference2;
-float proportion;
-float scaledDown;
-float W;
-float R;
-int dir; //1 CCW, 2 CW, 0 straight
-float heading = 0.0;
-float dispAngleRad;
-float dispAngleDeg;
-float x;
-float dx;
-float y;
-float dy;
+void 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;
 
-float totalDistance;
-int i;
-
-
+    if (dir == STRAIGHT) {
+        x = 0;
+        y = distance;
+    } else if (dir == CCW) {
+        x = -1*(R*(1-cos(dispAngle)));
+        y = R*sin(dispAngle);
+        /*
+        switch(quadrant) {
+            case Q1:
+                x = R*(cos(dispAngle)-1);
+                y = R*sin(dispAngle);
+                break;
+            case Q2:
+                x = -1*R*sin(dispAngle);
+                y = R*(cos(dispAngle)-1);
+                break;
+            case Q3:
+                x = R*(cos(dispAngle)-1);
+                y = -1*R*sin(dispAngle);
+                break;
+            case Q4:
+                x = R*sin(dispAngle);
+                y = R*(1-cos(dispAngle));
+                break;
+            default:
+                pc.printf("default \n\r");
+        }*/
+    } else if (dir == CW) {
+        x = R*(1-cos(dispAngle));
+        y = R*sin(dispAngle);
+        /*
+        switch(quadrant) {
+            case Q1:
+                x = R*sin(dispAngle);
+                y = R*(cos(dispAngle)-1);
+                break;
+            case Q2:
+                x = R*(cos(dispAngle)-1);
+                y = R*sin(dispAngle);
+                break;
+            case Q3:
+                x = -1*R*sin(dispAngle);
+                y = R*(1-cos(dispAngle));
+                break;
+            case Q4:
+                x = R*(cos(dispAngle)-1);
+                y = -1*R*sin(dispAngle);
+                break;
+            default:
+                pc.printf("default \n\r");
+        }*/
+    } else {
+        pc.printf("Direction not set. TERMINATED. \n\r");
+        while(true) {}
+    }
 
-void measure() {
-    // raw measurements of pot, encoder
-    //(pulse count / X * N) * (1 / PPI)
-    pulseCount = (float) wheel.getPulses();
-    /* conversion to relevant values: distance of arc traveled, angle of turn
-        |   suppose the truck is a two-segment body
-        /  <- angle between bottom leg and the vertical = angle of turn
-    the 20 and 68 were arbitrary and determined from testing, respectively
-    the distance is in inches
-    */
-    distance = pulseCount*circumference/encoding/20/68; 
-    // with half inch correction after testing extensively
-    /*if (distance > 0) {                                 
-        distance-=0.5;
-    } 
-    */
+    wait(.0001);
 }
 
-/* VERSION 1
-void calculateDisplacement() {
-    /
-        Premise:
-        Suppose the arc s (distance), the distance traveled by the 
-        turning truck, is defined by two connected points on 
-        a circle with radius r.
-    /
-    
-    // convert angle to radians
-    angle = angle*PI/180;
-    // r = s/angle, for distance of arc s         
-    r = distance/(angle);
-    // c = 2*r*sin(angle/2) for chord c
-    c = 2*r*sin(angle/2);
-    // intermediate value
-    segment = (r*(1-cos(angle)));
-    // calculation of angle of displacement
-    dispAngle = acos(segment/c);
-    // calculation of displacement
-    dx = c*cos(dispAngle+heading);
-    dy = c*sin(dispAngle+heading);
-    x = x - r + dx;
-    y = y + dy;
-    wait(1.0);                                           
-}
-*/
-
-void calculateDisplacement() {
-    df = 4.5; // distance from pivot to front wheel
-    db = 4.25; // distance from pivot to back wheel
-    // W = 2*R*sin(0.5*angle);
-    // W = shortest distance between wheels
-    angle = angle*PI/180;
-    W = sqrt(df*df+db*db-2*df*db*cos(PI-angle));
-    R = W/(2*sin(0.5*angle));
-    
-    circumference2 = 2*PI*R; // inches
-    proportion = distance/circumference2;
-    scaledDown = proportion - floor(proportion);
-    dispAngleRad = scaledDown*2*PI;
-    dispAngleDeg = scaledDown*360;
-    dx = R*cos(dispAngleRad + heading);
-    dy = R*sin(dispAngleRad + heading);
-
-    if(dir == 1) {               // CCW
-        x = 0 - R + dx;
-        y = dy;
-    } else if (dir == 2) {                // CW
-        y = dy;
-        if((dispAngleDeg < 90) || (dispAngleDeg > 270)) {
-        x = dx;
-        } else {
-            x = R - dx;
-        }
-    } else {
-        x = 0;
-        y = distance;
+int main() {
+    clear();
+    potSetup();
+    setDirection();
+    setQuadrant();
+    checkToStart();
+    while(true) {
+        calcDisplacement();
+        pc.printf("----------------------\n\r");
+        pc.printf("x: %f\n\r", x);
+        pc.printf("y: %f\n\r", y);
+        pc.printf("distance: %f\n\r", distance);
+        pc.printf("turnAngle: %f\n\r", turnAngle);
+        pc.printf("dispAngle: %f\n\r", dispAngle);
+        pc.printf("direction: %d\n\r", dir);
+        pc.printf("quadrant: %d\n\r", quadrant);
+        pc.printf("W: %f\n\r", W);
+        pc.printf("R: %f\n\r", R);
     }
-    heading += dispAngleRad;
-    wait(1.0);
-}
-/*
-dir
-and straight accuracy
-*/
-
-int main() {
-    // required to set truck straight before start up to calibrate to zero
-    zero = (float) pot;
-    while(i<5) {
-        printf("%d set angle \n\r",i);
-        wait(1);
-        i++;
-    }
-    if (totalDistance == 0) { // pot turns constant after the distance turns non zero
-        value = (float) pot;
-        // appropriate conversion from potentiometer float value to angle
-        angle = (value-zero)*300;
-        printf("%f",angle);
-    }
-    distance = 0;
-    totalDistance = 0;
-    heading = 0;
-    while(1){
-        measure(); 
-
-        calculateDisplacement();
-        
-        totalDistance+=distance;
-        if(totalDistance>0) {//@@@@@@@@@@check dir with angle vals@@@@@@@
-            if (angle > 0.5) {
-                dir = 1;//ccw
-            } else if (angle < -0.5) {
-                dir = 2;//cw
-            } else {
-                dir = 0;
-            }
-        }
-        distance = 0;
-        
-        /*
-            Notes:
-            At startup:
-            distance = 0
-            angle =! 0
-        */
-        
-        // for debugging
-        pc.printf("Distance %f, Angle %f\n\r", totalDistance, angle*180/PI);     
-        pc.printf("(X,Y): %f %f\n\r", x, y); 
-        pc.printf("(DX,DY): %f %f\n\r", dx, dy);    
-        pc.printf("heading: %f\n\r", heading);
-        pc.printf("----------------------------\n\r");   
-    }
-}
-
-
+}
\ No newline at end of file