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