File content as of revision 14:82248fb06e53:

#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;
}