The localization library for SCRIBE
localization.cpp@4:5c1de47812db, 2016-04-20 (annotated)
- Committer:
- nibab
- Date:
- Wed Apr 20 00:31:22 2016 +0000
- Revision:
- 4:5c1de47812db
- Parent:
- 2:e9d692eea8de
- Child:
- 5:dcce3f262830
Comment out the ultrasound parts.
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 | 4:5c1de47812db | 11 | //ultrasonic mu1(p28, p27, .1, 1, &dist); |
nibab | 4:5c1de47812db | 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 | 4:5c1de47812db | 20 | //mu1.startUpdates();//start mesuring the distance |
nibab | 4:5c1de47812db | 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 | 1:2cd9602780f4 | 45 | float localization::getAngle(){ |
nibab | 1:2cd9602780f4 | 46 | #ifndef VERTICAL //On horizontal plane |
nibab | 1:2cd9602780f4 | 47 | imu.setmode(OPERATION_MODE_NDOF); |
nibab | 1:2cd9602780f4 | 48 | imu.get_angles(); |
nibab | 1:2cd9602780f4 | 49 | float angle = imu.euler.yaw; |
nibab | 1:2cd9602780f4 | 50 | return angle; |
nibab | 1:2cd9602780f4 | 51 | #else // On vertical plane |
nibab | 1:2cd9602780f4 | 52 | imu.setmode(OPERATION_MODE_NDOF); |
nibab | 1:2cd9602780f4 | 53 | imu.get_grv(); |
nibab | 1:2cd9602780f4 | 54 | float gravityX = imu.gravity.x, gravityY = imu.gravity.y; |
nibab | 1:2cd9602780f4 | 55 | float radian = atan(gravityY/gravityX); |
nibab | 1:2cd9602780f4 | 56 | float degree = radian * 180 / 3.14159265359; |
nibab | 1:2cd9602780f4 | 57 | if(gravityX >= 0 && gravityY >= 0) |
nibab | 1:2cd9602780f4 | 58 | return degree; |
nibab | 1:2cd9602780f4 | 59 | else if(gravityX < 0) |
nibab | 1:2cd9602780f4 | 60 | return 180 + degree; |
nibab | 1:2cd9602780f4 | 61 | else // gravityX >= 0 && gravityY < 0 |
nibab | 1:2cd9602780f4 | 62 | return 360 + degree; |
nibab | 1:2cd9602780f4 | 63 | #endif |
nibab | 1:2cd9602780f4 | 64 | } |
nibab | 1:2cd9602780f4 | 65 | |
nibab | 4:5c1de47812db | 66 | /* |
nibab | 0:a5cac0a5e41d | 67 | void localization::measure(){ |
nibab | 0:a5cac0a5e41d | 68 | while(1){ |
nibab | 0:a5cac0a5e41d | 69 | imu.setmode(OPERATION_MODE_NDOF); |
nibab | 0:a5cac0a5e41d | 70 | imu.get_angles(); |
nibab | 2:e9d692eea8de | 71 | float angle = this->getAngle(); |
nibab | 2:e9d692eea8de | 72 | printf("Angle: %.3f\r\n", angle); |
nibab | 2:e9d692eea8de | 73 | if(angle < 359.0 && angle >= 180.0){ |
nibab | 0:a5cac0a5e41d | 74 | //turn right here |
nibab | 0:a5cac0a5e41d | 75 | printf("Turn right!\r\n"); |
nibab | 0:a5cac0a5e41d | 76 | wait(0.1); |
nibab | 0:a5cac0a5e41d | 77 | continue; |
nibab | 2:e9d692eea8de | 78 | }else if(angle >= 1.0 && angle < 180.0){ |
nibab | 0:a5cac0a5e41d | 79 | //turn left here |
nibab | 0:a5cac0a5e41d | 80 | printf("Turn left!\r\n"); |
nibab | 0:a5cac0a5e41d | 81 | wait(0.1); |
nibab | 0:a5cac0a5e41d | 82 | continue; |
nibab | 0:a5cac0a5e41d | 83 | }else{break;} |
nibab | 0:a5cac0a5e41d | 84 | } |
nibab | 2:e9d692eea8de | 85 | |
nibab | 0:a5cac0a5e41d | 86 | //mu1.checkDistance(); //call checkDistance() as much as possible, as this is where |
nibab | 0:a5cac0a5e41d | 87 | //the class checks if dist needs to be called. |
nibab | 0:a5cac0a5e41d | 88 | |
nibab | 0:a5cac0a5e41d | 89 | int sum1 = 0, sum2 = 0; |
nibab | 0:a5cac0a5e41d | 90 | for(int i = 0; i < 50; i++){ // Average over 10 times |
nibab | 0:a5cac0a5e41d | 91 | sum1 += mu1.getCurrentDistance(); |
nibab | 0:a5cac0a5e41d | 92 | sum2 += mu2.getCurrentDistance(); |
nibab | 0:a5cac0a5e41d | 93 | } |
nibab | 0:a5cac0a5e41d | 94 | X = sum1/50; |
nibab | 0:a5cac0a5e41d | 95 | Y = sum2/50; |
nibab | 0:a5cac0a5e41d | 96 | //printf("X is %dmm\r\n", X); |
nibab | 0:a5cac0a5e41d | 97 | //printf("Y is %dmm\r\n", Y); |
nibab | 0:a5cac0a5e41d | 98 | //wait(0.1); |
nibab | 0:a5cac0a5e41d | 99 | } |
nibab | 4:5c1de47812db | 100 | */ |
nibab | 0:a5cac0a5e41d | 101 | |
nibab | 0:a5cac0a5e41d | 102 | int localization::getX(){ |
nibab | 0:a5cac0a5e41d | 103 | return X; |
nibab | 0:a5cac0a5e41d | 104 | } |
nibab | 0:a5cac0a5e41d | 105 | |
nibab | 0:a5cac0a5e41d | 106 | int localization::getY(){ |
nibab | 0:a5cac0a5e41d | 107 | return Y; |
nibab | 0:a5cac0a5e41d | 108 | } |