Diff: localization.cpp

Revision:

11:59a95866416e

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/localization.cpp	Thu May 05 22:01:18 2016 +0000
@@ -0,0 +1,113 @@
+#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(p28, p27); 
+ultrasonic mu1(p26, p25, .1, 1, &dist);
+ultrasonic mu2(p30, p29, .1, 1, &dist);
+PwmOut pin24(p24);
+
+localization::localization(){
+    X = 0;
+    Y = 0;
+}
+
+void localization::reset(){
+    pin24.period_ms(20);
+    
+    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");
+}
+
+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(){
+    
+    while(1){
+        float angle = this->getAngle();
+        printf("Angle: %.3f\r\n", angle);
+        if(angle < 359.0 && angle >= 180.0){
+            step_right();
+            printf("Turning angle: %f \r\n", 360 - this->getAngle());
+        }else if(angle >= 1.0 && angle < 180.0){
+            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 50 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);
+}
+
+void localization::servo(int degree){
+    /*Input have to be between 0 and 180*/
+    if (degree > 180)
+        degree = 180;
+    else if (degree < 0)
+        degree = 0;
+    int highTime_us = (int)((degree / 180.0) * 2000) + 500; // Minimum 500us, maximum 2500us
+    
+    pin24.pulsewidth_us(highTime_us);  //Set the duty cycle
+}
+
+int localization::getX(){
+    return X;
+}
+
+int localization::getY(){
+    return Y;
+}