![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
The servo version of SCRIBE
Dependencies: BLE_nRF8001 BNO055 HC_SR04_Ultrasonic_Library mbed-rtos mbed
Fork of SCRIBE_stepper by
localization.cpp@10:c58f3a00a9ba, 2016-05-05 (annotated)
- Committer:
- nibab
- Date:
- Thu May 05 21:55:28 2016 +0000
- Revision:
- 10:c58f3a00a9ba
Get rid of the localization library.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
nibab | 10:c58f3a00a9ba | 1 | #include "localization.h" |
nibab | 10:c58f3a00a9ba | 2 | |
nibab | 10:c58f3a00a9ba | 3 | |
nibab | 10:c58f3a00a9ba | 4 | void dist(int distance) |
nibab | 10:c58f3a00a9ba | 5 | { |
nibab | 10:c58f3a00a9ba | 6 | //put code here to happen when the distance is changed |
nibab | 10:c58f3a00a9ba | 7 | printf("Distance1 changed to %dmm\r\n", distance); |
nibab | 10:c58f3a00a9ba | 8 | } |
nibab | 10:c58f3a00a9ba | 9 | |
nibab | 10:c58f3a00a9ba | 10 | BNO055 imu(p28, p27); |
nibab | 10:c58f3a00a9ba | 11 | ultrasonic mu1(p26, p25, .1, 1, &dist); |
nibab | 10:c58f3a00a9ba | 12 | ultrasonic mu2(p30, p29, .1, 1, &dist); |
nibab | 10:c58f3a00a9ba | 13 | PwmOut pin24(p24); |
nibab | 10:c58f3a00a9ba | 14 | |
nibab | 10:c58f3a00a9ba | 15 | localization::localization(){ |
nibab | 10:c58f3a00a9ba | 16 | X = 0; |
nibab | 10:c58f3a00a9ba | 17 | Y = 0; |
nibab | 10:c58f3a00a9ba | 18 | } |
nibab | 10:c58f3a00a9ba | 19 | |
nibab | 10:c58f3a00a9ba | 20 | void localization::reset(){ |
nibab | 10:c58f3a00a9ba | 21 | pin24.period_ms(20); |
nibab | 10:c58f3a00a9ba | 22 | |
nibab | 10:c58f3a00a9ba | 23 | mu1.startUpdates();//start mesuring the distance |
nibab | 10:c58f3a00a9ba | 24 | mu2.startUpdates(); |
nibab | 10:c58f3a00a9ba | 25 | imu.reset(); |
nibab | 10:c58f3a00a9ba | 26 | // Check that BNO055 is connected |
nibab | 10:c58f3a00a9ba | 27 | if (!imu.check()){ |
nibab | 10:c58f3a00a9ba | 28 | while (true){ |
nibab | 10:c58f3a00a9ba | 29 | printf("BNO055 IMU not connected"); |
nibab | 10:c58f3a00a9ba | 30 | wait(0.1); |
nibab | 10:c58f3a00a9ba | 31 | } |
nibab | 10:c58f3a00a9ba | 32 | } |
nibab | 10:c58f3a00a9ba | 33 | // Display sensor information |
nibab | 10:c58f3a00a9ba | 34 | printf("BNO055 found\r\n\r\n"); |
nibab | 10:c58f3a00a9ba | 35 | printf("Chip ID: %d\r\n",imu.ID.id); |
nibab | 10:c58f3a00a9ba | 36 | printf("Accelerometer ID: %d\r\n",imu.ID.accel); |
nibab | 10:c58f3a00a9ba | 37 | printf("Gyroscope ID: %d\r\n",imu.ID.gyro); |
nibab | 10:c58f3a00a9ba | 38 | printf("Magnetometer ID: %d\r\n\r\n",imu.ID.mag); |
nibab | 10:c58f3a00a9ba | 39 | printf("Firmware version v%d.%0d\r\n",imu.ID.sw[0],imu.ID.sw[1]); |
nibab | 10:c58f3a00a9ba | 40 | printf("Bootloader version v%d\r\n\r\n",imu.ID.bootload); |
nibab | 10:c58f3a00a9ba | 41 | // Display chip serial number |
nibab | 10:c58f3a00a9ba | 42 | for (int i = 0; i<4; i++){ |
nibab | 10:c58f3a00a9ba | 43 | 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 | 10:c58f3a00a9ba | 44 | } |
nibab | 10:c58f3a00a9ba | 45 | printf("\r\n"); |
nibab | 10:c58f3a00a9ba | 46 | } |
nibab | 10:c58f3a00a9ba | 47 | |
nibab | 10:c58f3a00a9ba | 48 | float localization::getAngle(){ |
nibab | 10:c58f3a00a9ba | 49 | #ifndef VERTICAL //On horizontal plane |
nibab | 10:c58f3a00a9ba | 50 | imu.setmode(OPERATION_MODE_NDOF); |
nibab | 10:c58f3a00a9ba | 51 | imu.get_angles(); |
nibab | 10:c58f3a00a9ba | 52 | float angle = imu.euler.yaw; |
nibab | 10:c58f3a00a9ba | 53 | return angle; |
nibab | 10:c58f3a00a9ba | 54 | #else // On vertical plane |
nibab | 10:c58f3a00a9ba | 55 | imu.setmode(OPERATION_MODE_NDOF); |
nibab | 10:c58f3a00a9ba | 56 | imu.get_grv(); |
nibab | 10:c58f3a00a9ba | 57 | float gravityX = imu.gravity.x, gravityY = imu.gravity.y; |
nibab | 10:c58f3a00a9ba | 58 | float radian = atan(gravityY/gravityX); |
nibab | 10:c58f3a00a9ba | 59 | float degree = radian * 180 / 3.14159265359; |
nibab | 10:c58f3a00a9ba | 60 | if(gravityX >= 0 && gravityY >= 0) |
nibab | 10:c58f3a00a9ba | 61 | return degree; |
nibab | 10:c58f3a00a9ba | 62 | else if(gravityX < 0) |
nibab | 10:c58f3a00a9ba | 63 | return 180 + degree; |
nibab | 10:c58f3a00a9ba | 64 | else // gravityX >= 0 && gravityY < 0 |
nibab | 10:c58f3a00a9ba | 65 | return 360 + degree; |
nibab | 10:c58f3a00a9ba | 66 | #endif |
nibab | 10:c58f3a00a9ba | 67 | } |
nibab | 10:c58f3a00a9ba | 68 | |
nibab | 10:c58f3a00a9ba | 69 | void localization::measure(){ |
nibab | 10:c58f3a00a9ba | 70 | |
nibab | 10:c58f3a00a9ba | 71 | while(1){ |
nibab | 10:c58f3a00a9ba | 72 | float angle = this->getAngle(); |
nibab | 10:c58f3a00a9ba | 73 | printf("Angle: %.3f\r\n", angle); |
nibab | 10:c58f3a00a9ba | 74 | if(angle < 359.0 && angle >= 180.0){ |
nibab | 10:c58f3a00a9ba | 75 | step_right(); |
nibab | 10:c58f3a00a9ba | 76 | printf("Turning angle: %f \r\n", 360 - this->getAngle()); |
nibab | 10:c58f3a00a9ba | 77 | }else if(angle >= 1.0 && angle < 180.0){ |
nibab | 10:c58f3a00a9ba | 78 | step_left(); |
nibab | 10:c58f3a00a9ba | 79 | printf("Turning angle: %f \r\n", this->getAngle()); |
nibab | 10:c58f3a00a9ba | 80 | }else{break;} |
nibab | 10:c58f3a00a9ba | 81 | } |
nibab | 10:c58f3a00a9ba | 82 | //mu1.checkDistance(); //call checkDistance() as much as possible, as this is where |
nibab | 10:c58f3a00a9ba | 83 | //the class checks if dist needs to be called. |
nibab | 10:c58f3a00a9ba | 84 | |
nibab | 10:c58f3a00a9ba | 85 | int sum1 = 0, sum2 = 0; |
nibab | 10:c58f3a00a9ba | 86 | for(int i = 0; i < 50; i++){ // Average over 50 times |
nibab | 10:c58f3a00a9ba | 87 | sum1 += mu1.getCurrentDistance(); |
nibab | 10:c58f3a00a9ba | 88 | sum2 += mu2.getCurrentDistance(); |
nibab | 10:c58f3a00a9ba | 89 | } |
nibab | 10:c58f3a00a9ba | 90 | X = sum1/50; |
nibab | 10:c58f3a00a9ba | 91 | Y = sum2/50; |
nibab | 10:c58f3a00a9ba | 92 | //printf("X is %dmm\r\n", X); |
nibab | 10:c58f3a00a9ba | 93 | //printf("Y is %dmm\r\n", Y); |
nibab | 10:c58f3a00a9ba | 94 | } |
nibab | 10:c58f3a00a9ba | 95 | |
nibab | 10:c58f3a00a9ba | 96 | void localization::servo(int degree){ |
nibab | 10:c58f3a00a9ba | 97 | /*Input have to be between 0 and 180*/ |
nibab | 10:c58f3a00a9ba | 98 | if (degree > 180) |
nibab | 10:c58f3a00a9ba | 99 | degree = 180; |
nibab | 10:c58f3a00a9ba | 100 | else if (degree < 0) |
nibab | 10:c58f3a00a9ba | 101 | degree = 0; |
nibab | 10:c58f3a00a9ba | 102 | int highTime_us = (int)((degree / 180.0) * 2000) + 500; // Minimum 500us, maximum 2500us |
nibab | 10:c58f3a00a9ba | 103 | |
nibab | 10:c58f3a00a9ba | 104 | pin24.pulsewidth_us(highTime_us); //Set the duty cycle |
nibab | 10:c58f3a00a9ba | 105 | } |
nibab | 10:c58f3a00a9ba | 106 | |
nibab | 10:c58f3a00a9ba | 107 | int localization::getX(){ |
nibab | 10:c58f3a00a9ba | 108 | return X; |
nibab | 10:c58f3a00a9ba | 109 | } |
nibab | 10:c58f3a00a9ba | 110 | |
nibab | 10:c58f3a00a9ba | 111 | int localization::getY(){ |
nibab | 10:c58f3a00a9ba | 112 | return Y; |
nibab | 10:c58f3a00a9ba | 113 | } |