Revision 8:91db52943f3e, committed 2016-04-25

Comitter:

nibab

Date:

Mon Apr 25 01:04:51 2016 +0000

Parent:

7:fcdba69a0c22

Commit message:

Automatically turn to the original position and measure. #define MMPERSTEP 17.

Changed in this revision

--- 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
+}

--- a/localization.h	Fri Apr 22 18:30:52 2016 +0000
+++ b/localization.h	Mon Apr 25 01:04:51 2016 +0000
@@ -1,6 +1,8 @@
 #include "mbed.h"
 #include "ultrasonic.h"
 #include "BNO055.h"
+#include "stepper.h"
+#define MMPERSTEP 17
 
 //#define VERTICAL // define it on the vertical version to compile the corresponding code