The localization library for SCRIBE
localization.cpp
- Committer:
- nibab
- Date:
- 2016-04-22
- Revision:
- 7:fcdba69a0c22
- Parent:
- 6:ee872bf49be6
- Child:
- 8:91db52943f3e
File content as of revision 7:fcdba69a0c22:
#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){ imu.setmode(OPERATION_MODE_NDOF); imu.get_angles(); float angle = this->getAngle(); printf("Angle: %.3f\r\n", angle); if(angle < 359.0 && angle >= 180.0){ //turn right here printf("Turn right!\r\n"); wait(0.1); continue; }else if(angle >= 1.0 && angle < 180.0){ //turn left here printf("Turn left!\r\n"); wait(0.1); continue; }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 10 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); //wait(0.1); } 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; }