Diff: localization.cpp

Revision:

0:a5cac0a5e41d

Child:

1:2cd9602780f4

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/localization.cpp	Mon Apr 18 21:05:04 2016 +0000
@@ -0,0 +1,106 @@
+#include "localization.h"
+
+
+void dist(int distance)
+{
+    //put code here to happen when the distance is changed
+    printf("Distance1 changed to %dmm\r\n", distance);
+}
+
+BNO055 imu(p9, p10); 
+ultrasonic mu1(p28, p27, .1, 1, &dist);
+ultrasonic mu2(p30, p29, .1, 1, &dist);
+
+localization::localization(){
+    X = 0;
+    Y = 0;
+}
+
+void localization::reset(){
+    mu1.startUpdates();//start mesuring the distance
+    mu2.startUpdates();
+    imu.reset();
+    // Check that BNO055 is connected
+    if (!imu.check()){
+        while (true){
+            printf("BNO055 IMU not connected");
+            wait(0.1);
+        }
+    }
+    // Display sensor information
+    printf("BNO055 found\r\n\r\n");
+    printf("Chip          ID: %d\r\n",imu.ID.id);
+    printf("Accelerometer ID: %d\r\n",imu.ID.accel);
+    printf("Gyroscope     ID: %d\r\n",imu.ID.gyro);
+    printf("Magnetometer  ID: %d\r\n\r\n",imu.ID.mag);
+    printf("Firmware version v%d.%0d\r\n",imu.ID.sw[0],imu.ID.sw[1]);
+    printf("Bootloader version v%d\r\n\r\n",imu.ID.bootload);
+    // Display chip serial number
+    for (int i = 0; i<4; i++){
+        printf("%d.%d.%d.%d\r\n",imu.ID.serial[i*4],imu.ID.serial[i*4+1],imu.ID.serial[i*4+2],imu.ID.serial[i*4+3]);
+    }
+    printf("\r\n");
+}
+
+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){
+            //turn right here
+            printf("Turn right!\r\n");
+            wait(0.1);
+            continue;
+        }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 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){
+                //turn right here
+                printf("Turn right!\r\n");
+                wait(0.1);
+                continue;
+            }else if(Y > 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.
+        
+    int sum1 = 0, sum2 = 0;
+    for(int i = 0; i < 50; i++){ // Average over 10 times
+        sum1 += mu1.getCurrentDistance();
+        sum2 += mu2.getCurrentDistance();
+    }
+    X = sum1/50;
+    Y = sum2/50;  
+    //printf("X is %dmm\r\n", X);
+    //printf("Y is %dmm\r\n", Y);
+    //wait(0.1);
+}
+
+int localization::getX(){
+    return X;
+}
+
+int localization::getY(){
+    return Y;
+}