SCRIBE / Mbed 2 deprecated SCRIBE_servo

The servo version of SCRIBE

Dependencies:   BLE_nRF8001 BNO055 HC_SR04_Ultrasonic_Library mbed-rtos mbed

Fork of SCRIBE_stepper by SCRIBE

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?

UserRevisionLine numberNew 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 }