Diff: localization.cpp

Revision:

2:e9d692eea8de

Parent:

1:2cd9602780f4

Child:

4:5c1de47812db

--- a/localization.cpp	Mon Apr 18 22:46:58 2016 +0000
+++ b/localization.cpp	Mon Apr 18 23:05:21 2016 +0000
@@ -64,45 +64,24 @@
 }
 
 void localization::measure(){
-    #ifndef VERTICAL //On horizontal plane
     while(1){
         imu.setmode(OPERATION_MODE_NDOF);
         imu.get_angles();
-        float angle = imu.euler.yaw;
-        //printf("Angle: %.3f\r\n", angle);
-        if(angle < 359.0 && angle > 180.0){
+        float angle = this->getAngle();
+        printf("Angle: %.3f\r\n", angle);
+        if(angle < 359.0 && angle >= 180.0){
             //turn right here
             printf("Turn right!\r\n");
             wait(0.1);
             continue;
-        }else if(angle > 1.0 && angle < 180.0){
+        }else if(angle >= 1.0 && angle < 180.0){
             //turn left here
             printf("Turn left!\r\n");
             wait(0.1);
             continue;
         }else{break;}
     }
-    #else // On vertical plane
-    while(1){
-        imu.setmode(OPERATION_MODE_NDOF);
-        imu.get_grv();
-        float gravityX = imu.gravity.x, gravityY = imu.gravity.y;
-        printf("Gravity x: %.3f, y: %.3f\r\n", gravityX, gravityY);
-        if(gravityX < 0.975){ // Not facing up
-            if(gravityY < 0){
-                //turn right here
-                printf("Turn right!\r\n");
-                wait(0.1);
-                continue;
-            }else if(gravityY >= 0){
-                //turn left here
-                printf("Turn left!\r\n");
-                wait(0.1);
-                continue;
-            }
-        }else{break;}
-    }
-    #endif
+
     //mu1.checkDistance();     //call checkDistance() as much as possible, as this is where
                                 //the class checks if dist needs to be called.