Terrabots / Mbed 2 deprecated TerraBot_Drive_2D_FINAL

2d drive position tracking (final draft) (CLEANED UP)

Dependencies:   MotorV2 QEI mbed

Fork of TerraBot_Drive_2D by Terrabots

Files at this revision

API Documentation at this revision

Revision 2:e68fd2af041d, committed 2016-12-05

Comitter:
simplyellow
Date:
Mon Dec 05 22:56:32 2016 +0000
Parent:
1:17db90f61f6c
Commit message:
cleaned up final draft

Changed in this revision

main.cpp Show annotated file Show diff for this revision Revisions of this file 
diff -r 17db90f61f6c -r e68fd2af041d main.cpp
--- a/main.cpp	Tue Nov 29 21:16:58 2016 +0000
+++ b/main.cpp	Mon Dec 05 22:56:32 2016 +0000
@@ -36,13 +36,6 @@
 float R;
 float dispAngle;
 
-#define Q1 1
-#define Q2 2
-#define Q3 3
-#define Q4 4
-
-int quadrant = 0;
-
 void clear() {
     distance = 0;
     x = 0;
@@ -82,23 +75,6 @@
     turnAngle = abs(turnAngle);
 }
 
-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) {}
-    }
-}
-
 void checkToStart() {               // start tracking when distance > 0
     while(true) {
         pulseCount = (float) wheel.getPulses();
@@ -110,13 +86,6 @@
         }
     }
 }
-/*
-AXES
-when robot is facing forward
-forward is +x
-left is +y
-*/
-
 
 void calcDisplacement() {
     pulseCount = (float) wheel.getPulses();
@@ -132,51 +101,9 @@
     } 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) {}