Diff: localization.cpp

Revision:

1:2cd9602780f4

Parent:

0:a5cac0a5e41d

Child:

2:e9d692eea8de

--- a/localization.cpp	Mon Apr 18 21:05:04 2016 +0000
+++ b/localization.cpp	Mon Apr 18 22:46:58 2016 +0000
@@ -42,13 +42,34 @@
     printf("\r\n");
 }
 
+float localization::getAngle(){
+    #ifndef VERTICAL //On horizontal plane
+    imu.setmode(OPERATION_MODE_NDOF);
+    imu.get_angles();
+    float angle = imu.euler.yaw;
+    return angle;
+    #else // On vertical plane
+    imu.setmode(OPERATION_MODE_NDOF);
+    imu.get_grv();
+    float gravityX = imu.gravity.x, gravityY = imu.gravity.y;
+    float radian = atan(gravityY/gravityX);
+    float degree = radian * 180 / 3.14159265359;
+    if(gravityX >= 0 && gravityY >= 0)
+        return degree;
+    else if(gravityX < 0)
+        return 180 + degree;
+    else // gravityX >= 0 && gravityY < 0
+        return 360 + degree;
+    #endif
+}
+
 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);
+        //printf("Angle: %.3f\r\n", angle);
         if(angle < 359.0 && angle > 180.0){
             //turn right here
             printf("Turn right!\r\n");
@@ -65,15 +86,15 @@
     while(1){
         imu.setmode(OPERATION_MODE_NDOF);
         imu.get_grv();
-        float X = imu.gravity.x, Y = imu.gravity.y;
-        printf("Gravity x: %.3f, y: %.3f\r\n", X, Y);
-        if(X < 0.975){ // Not facing up
-            if(Y < 0){
+        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(Y > 0){
+            }else if(gravityY >= 0){
                 //turn left here
                 printf("Turn left!\r\n");
                 wait(0.1);