SCRIBE / localization

The localization library for SCRIBE

Dependents:   SCRIBE_stepper

localization.cpp@0:a5cac0a5e41d, 2016-04-18 (annotated)

Committer:
nibab
Date:
Mon Apr 18 21:05:04 2016 +0000
Revision:
0:a5cac0a5e41d
Child:
1:2cd9602780f4
Made localization a library

Who changed what in which revision?

UserRevisionLine numberNew contents of line
nibab 0:a5cac0a5e41d 1 #include "localization.h"
nibab 0:a5cac0a5e41d 2
nibab 0:a5cac0a5e41d 3
nibab 0:a5cac0a5e41d 4 void dist(int distance)
nibab 0:a5cac0a5e41d 5 {
nibab 0:a5cac0a5e41d 6 //put code here to happen when the distance is changed
nibab 0:a5cac0a5e41d 7 printf("Distance1 changed to %dmm\r\n", distance);
nibab 0:a5cac0a5e41d 8 }
nibab 0:a5cac0a5e41d 9
nibab 0:a5cac0a5e41d 10 BNO055 imu(p9, p10);
nibab 0:a5cac0a5e41d 11 ultrasonic mu1(p28, p27, .1, 1, &dist);
nibab 0:a5cac0a5e41d 12 ultrasonic mu2(p30, p29, .1, 1, &dist);
nibab 0:a5cac0a5e41d 13
nibab 0:a5cac0a5e41d 14 localization::localization(){
nibab 0:a5cac0a5e41d 15 X = 0;
nibab 0:a5cac0a5e41d 16 Y = 0;
nibab 0:a5cac0a5e41d 17 }
nibab 0:a5cac0a5e41d 18
nibab 0:a5cac0a5e41d 19 void localization::reset(){
nibab 0:a5cac0a5e41d 20 mu1.startUpdates();//start mesuring the distance
nibab 0:a5cac0a5e41d 21 mu2.startUpdates();
nibab 0:a5cac0a5e41d 22 imu.reset();
nibab 0:a5cac0a5e41d 23 // Check that BNO055 is connected
nibab 0:a5cac0a5e41d 24 if (!imu.check()){
nibab 0:a5cac0a5e41d 25 while (true){
nibab 0:a5cac0a5e41d 26 printf("BNO055 IMU not connected");
nibab 0:a5cac0a5e41d 27 wait(0.1);
nibab 0:a5cac0a5e41d 28 }
nibab 0:a5cac0a5e41d 29 }
nibab 0:a5cac0a5e41d 30 // Display sensor information
nibab 0:a5cac0a5e41d 31 printf("BNO055 found\r\n\r\n");
nibab 0:a5cac0a5e41d 32 printf("Chip ID: %d\r\n",imu.ID.id);
nibab 0:a5cac0a5e41d 33 printf("Accelerometer ID: %d\r\n",imu.ID.accel);
nibab 0:a5cac0a5e41d 34 printf("Gyroscope ID: %d\r\n",imu.ID.gyro);
nibab 0:a5cac0a5e41d 35 printf("Magnetometer ID: %d\r\n\r\n",imu.ID.mag);
nibab 0:a5cac0a5e41d 36 printf("Firmware version v%d.%0d\r\n",imu.ID.sw[0],imu.ID.sw[1]);
nibab 0:a5cac0a5e41d 37 printf("Bootloader version v%d\r\n\r\n",imu.ID.bootload);
nibab 0:a5cac0a5e41d 38 // Display chip serial number
nibab 0:a5cac0a5e41d 39 for (int i = 0; i<4; i++){
nibab 0:a5cac0a5e41d 40 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]);
nibab 0:a5cac0a5e41d 41 }
nibab 0:a5cac0a5e41d 42 printf("\r\n");
nibab 0:a5cac0a5e41d 43 }
nibab 0:a5cac0a5e41d 44
nibab 0:a5cac0a5e41d 45 void localization::measure(){
nibab 0:a5cac0a5e41d 46 #ifndef VERTICAL //On horizontal plane
nibab 0:a5cac0a5e41d 47 while(1){
nibab 0:a5cac0a5e41d 48 imu.setmode(OPERATION_MODE_NDOF);
nibab 0:a5cac0a5e41d 49 imu.get_angles();
nibab 0:a5cac0a5e41d 50 float angle = imu.euler.yaw;
nibab 0:a5cac0a5e41d 51 printf("Angle: %.3f\r\n", angle);
nibab 0:a5cac0a5e41d 52 if(angle < 359.0 && angle > 180.0){
nibab 0:a5cac0a5e41d 53 //turn right here
nibab 0:a5cac0a5e41d 54 printf("Turn right!\r\n");
nibab 0:a5cac0a5e41d 55 wait(0.1);
nibab 0:a5cac0a5e41d 56 continue;
nibab 0:a5cac0a5e41d 57 }else if(angle > 1.0 && angle < 180.0){
nibab 0:a5cac0a5e41d 58 //turn left here
nibab 0:a5cac0a5e41d 59 printf("Turn left!\r\n");
nibab 0:a5cac0a5e41d 60 wait(0.1);
nibab 0:a5cac0a5e41d 61 continue;
nibab 0:a5cac0a5e41d 62 }else{break;}
nibab 0:a5cac0a5e41d 63 }
nibab 0:a5cac0a5e41d 64 #else // On vertical plane
nibab 0:a5cac0a5e41d 65 while(1){
nibab 0:a5cac0a5e41d 66 imu.setmode(OPERATION_MODE_NDOF);
nibab 0:a5cac0a5e41d 67 imu.get_grv();
nibab 0:a5cac0a5e41d 68 float X = imu.gravity.x, Y = imu.gravity.y;
nibab 0:a5cac0a5e41d 69 printf("Gravity x: %.3f, y: %.3f\r\n", X, Y);
nibab 0:a5cac0a5e41d 70 if(X < 0.975){ // Not facing up
nibab 0:a5cac0a5e41d 71 if(Y < 0){
nibab 0:a5cac0a5e41d 72 //turn right here
nibab 0:a5cac0a5e41d 73 printf("Turn right!\r\n");
nibab 0:a5cac0a5e41d 74 wait(0.1);
nibab 0:a5cac0a5e41d 75 continue;
nibab 0:a5cac0a5e41d 76 }else if(Y > 0){
nibab 0:a5cac0a5e41d 77 //turn left here
nibab 0:a5cac0a5e41d 78 printf("Turn left!\r\n");
nibab 0:a5cac0a5e41d 79 wait(0.1);
nibab 0:a5cac0a5e41d 80 continue;
nibab 0:a5cac0a5e41d 81 }
nibab 0:a5cac0a5e41d 82 }else{break;}
nibab 0:a5cac0a5e41d 83 }
nibab 0:a5cac0a5e41d 84 #endif
nibab 0:a5cac0a5e41d 85 //mu1.checkDistance(); //call checkDistance() as much as possible, as this is where
nibab 0:a5cac0a5e41d 86 //the class checks if dist needs to be called.
nibab 0:a5cac0a5e41d 87
nibab 0:a5cac0a5e41d 88 int sum1 = 0, sum2 = 0;
nibab 0:a5cac0a5e41d 89 for(int i = 0; i < 50; i++){ // Average over 10 times
nibab 0:a5cac0a5e41d 90 sum1 += mu1.getCurrentDistance();
nibab 0:a5cac0a5e41d 91 sum2 += mu2.getCurrentDistance();
nibab 0:a5cac0a5e41d 92 }
nibab 0:a5cac0a5e41d 93 X = sum1/50;
nibab 0:a5cac0a5e41d 94 Y = sum2/50;
nibab 0:a5cac0a5e41d 95 //printf("X is %dmm\r\n", X);
nibab 0:a5cac0a5e41d 96 //printf("Y is %dmm\r\n", Y);
nibab 0:a5cac0a5e41d 97 //wait(0.1);
nibab 0:a5cac0a5e41d 98 }
nibab 0:a5cac0a5e41d 99
nibab 0:a5cac0a5e41d 100 int localization::getX(){
nibab 0:a5cac0a5e41d 101 return X;
nibab 0:a5cac0a5e41d 102 }
nibab 0:a5cac0a5e41d 103
nibab 0:a5cac0a5e41d 104 int localization::getY(){
nibab 0:a5cac0a5e41d 105 return Y;
nibab 0:a5cac0a5e41d 106 }