Diff: localization.cpp

Revision:

8:91db52943f3e

Parent:

7:fcdba69a0c22

diff -r fcdba69a0c22 -r 91db52943f3e localization.cpp
--- a/localization.cpp	Fri Apr 22 18:30:52 2016 +0000
+++ b/localization.cpp	Mon Apr 25 01:04:51 2016 +0000
@@ -67,30 +67,23 @@
 }
 
 void localization::measure(){
-    /*
+    
     while(1){
-        imu.setmode(OPERATION_MODE_NDOF);
-        imu.get_angles();
         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;
+            step_right();
+            printf("Turning angle: %f \r\n", 360 - this->getAngle());
         }else if(angle >= 1.0 && angle < 180.0){
-            //turn left here
-            printf("Turn left!\r\n");
-            wait(0.1);
-            continue;
+            step_left();
+            printf("Turning angle: %f \r\n", this->getAngle());
         }else{break;}
     }
-    */
     //mu1.checkDistance();     //call checkDistance() as much as possible, as this is where
                                 //the class checks if dist needs to be called.
         
     int sum1 = 0, sum2 = 0;
-    for(int i = 0; i < 50; i++){ // Average over 10 times
+    for(int i = 0; i < 50; i++){ // Average over 50 times
         sum1 += mu1.getCurrentDistance();
         sum2 += mu2.getCurrentDistance();
     }
@@ -98,7 +91,6 @@
     Y = sum2/50;
     //printf("X is %dmm\r\n", X);
     //printf("Y is %dmm\r\n", Y);
-    //wait(0.1);
 }
 
 void localization::servo(int degree){
@@ -118,4 +110,4 @@
 
 int localization::getY(){
     return Y;
-}
\ No newline at end of file
+}