![](/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
Diff: localization.cpp
- Revision:
- 10:c58f3a00a9ba
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/localization.cpp Thu May 05 21:55:28 2016 +0000 @@ -0,0 +1,113 @@ +#include "localization.h" + + +void dist(int distance) +{ + //put code here to happen when the distance is changed + printf("Distance1 changed to %dmm\r\n", distance); +} + +BNO055 imu(p28, p27); +ultrasonic mu1(p26, p25, .1, 1, &dist); +ultrasonic mu2(p30, p29, .1, 1, &dist); +PwmOut pin24(p24); + +localization::localization(){ + X = 0; + Y = 0; +} + +void localization::reset(){ + pin24.period_ms(20); + + mu1.startUpdates();//start mesuring the distance + mu2.startUpdates(); + imu.reset(); + // Check that BNO055 is connected + if (!imu.check()){ + while (true){ + printf("BNO055 IMU not connected"); + wait(0.1); + } + } + // Display sensor information + printf("BNO055 found\r\n\r\n"); + printf("Chip ID: %d\r\n",imu.ID.id); + printf("Accelerometer ID: %d\r\n",imu.ID.accel); + printf("Gyroscope ID: %d\r\n",imu.ID.gyro); + printf("Magnetometer ID: %d\r\n\r\n",imu.ID.mag); + printf("Firmware version v%d.%0d\r\n",imu.ID.sw[0],imu.ID.sw[1]); + printf("Bootloader version v%d\r\n\r\n",imu.ID.bootload); + // Display chip serial number + for (int i = 0; i<4; i++){ + 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]); + } + printf("\r\n"); +} + +float localization::getAngle(){ + #ifndef VERTICAL //On horizontal plane + imu.setmode(OPERATION_MODE_NDOF); + imu.get_angles(); + float angle = imu.euler.yaw; + return angle; + #else // On vertical plane + imu.setmode(OPERATION_MODE_NDOF); + imu.get_grv(); + float gravityX = imu.gravity.x, gravityY = imu.gravity.y; + float radian = atan(gravityY/gravityX); + float degree = radian * 180 / 3.14159265359; + if(gravityX >= 0 && gravityY >= 0) + return degree; + else if(gravityX < 0) + return 180 + degree; + else // gravityX >= 0 && gravityY < 0 + return 360 + degree; + #endif +} + +void localization::measure(){ + + while(1){ + float angle = this->getAngle(); + printf("Angle: %.3f\r\n", angle); + if(angle < 359.0 && angle >= 180.0){ + step_right(); + printf("Turning angle: %f \r\n", 360 - this->getAngle()); + }else if(angle >= 1.0 && angle < 180.0){ + step_left(); + printf("Turning angle: %f \r\n", this->getAngle()); + }else{break;} + } + //mu1.checkDistance(); //call checkDistance() as much as possible, as this is where + //the class checks if dist needs to be called. + + int sum1 = 0, sum2 = 0; + for(int i = 0; i < 50; i++){ // Average over 50 times + sum1 += mu1.getCurrentDistance(); + sum2 += mu2.getCurrentDistance(); + } + X = sum1/50; + Y = sum2/50; + //printf("X is %dmm\r\n", X); + //printf("Y is %dmm\r\n", Y); +} + +void localization::servo(int degree){ + /*Input have to be between 0 and 180*/ + if (degree > 180) + degree = 180; + else if (degree < 0) + degree = 0; + int highTime_us = (int)((degree / 180.0) * 2000) + 500; // Minimum 500us, maximum 2500us + + pin24.pulsewidth_us(highTime_us); //Set the duty cycle +} + +int localization::getX(){ + return X; +} + +int localization::getY(){ + return Y; +}