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@14:82248fb06e53, 2016-05-12 (annotated)

Committer:
manz
Date:
Thu May 12 06:17:49 2016 +0000
Revision:
14:82248fb06e53
Parent:
11:59a95866416e 
latest version;

Who changed what in which revision?

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