The localization library for SCRIBE
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?
User | Revision | Line number | New 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 | } |