The localization library for SCRIBE

Dependents:   SCRIBE_stepper

Files at this revision

API Documentation at this revision

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

localization.cpp Show annotated file Show diff for this revision Revisions of this file
localization.h Show annotated file Show diff for this revision Revisions of this file
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
+}
diff -r fcdba69a0c22 -r 91db52943f3e localization.h
--- 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