File content as of revision 0:603c28b75dc1:

/* mbed ROBOT Library, for SUTD evobot project, Generation 1
 * Copyright (c) 2013, SUTD
 * Author: Mark VanderMeulen
 *
 * Dec 18, 2013
 *
 * You may want to rewrite this entire library, as this one is only half-finished
 *
 * This library allows the user to use each module on the Evobot Generation 1
 * Functionality:
 *      -Bluetooth connection
 *      -MPU (accelerometer/gyroscope) connection
 *      -Orientation in the XY plane (using Z-axis gyroscope only)
 *      -Remote control functionality (via bluetooth)
 *
 * Future functions:
 *      -Switch to turn bluetooth on/off
 *      -Switch to turn MPU on/off (in case of connection errors with the MPU: restart and reconnect
 *      -Calculation of distance travelled (use integration of accelerometer, maybe use Kalman filter)
 *      -Access to the DMP (Digital motion processor) on the MPU to calculate 3D orientation.
 *      -RF mesh network so robots can connect to eachother
 *      -Camera (requires an add-on to the circuit board at the moment)
 */


#include "robot.h"
#include "math.h"
//*********************************CONSTRUCTOR*********************************//
Robot::Robot() : bt(tx_bt,rx_bt),
    rf24(PTD2, PTD3, PTD1, PTD0, PTD5, PTD4),
    mpu(PTE0, PTE1),
    myled(LED),
    btSwitch(PTE25),
    currentSensor(CURRENTSENSOR_PIN),
    irSensorL(irL),
    irSensorC(irC),
    irSensorR(irR),
    voltageSensor(VOLTAGESENSOR_PIN),
    PWMA(MOT_PWMA_PIN),
    PWMB(MOT_PWMB_PIN),
    AIN1(MOT_AIN1_PIN),
    AIN2(MOT_AIN2_PIN),
    BIN1(MOT_BIN1_PIN),
    BIN2(MOT_BIN2_PIN),
    STBY(MOT_STBY_PIN)
{

    stop();  //Initialize motors as stopped//

    btSwitch = 1;    //turn on bluetooth
    myled = 0;   //turn ON status LED (0 = on, 1 = 0ff)
    timeNext = 0;
    //currentAvg = 0;          /////////////REMOVE currentAvg LATER. TESTING PURPOSES ONLY////////

    target_angle = 0;    //direction we want the robot to be pointed
    angle_origin = 0;    //you can use this to modify the angle of origin
    origin = 0;          //the (x,y) location of the origin (this should be a point, not a double)
    rz = 0;          //The current rotation in the Z-axis
    dx = 0;          //The current displacement in the x-axis    (side-side)
    dy = 0;          //The current displacement in the y-axis    (forward-back)
    dz = 0;          //The current displacement in the z-axis    (up-down)

    AUTO_ORIENT = 1;     //robot will automatically orient iteslf using the gyroscope
    REMOTE_CONTROL = 1;  //robot can be controlled over bluetooth

    tt.start();   //start timer
    mpu.testConnection();
    wait(1);
    MPU_OK = 0;

    //initialize MPU
    if (mpu.testConnection()) {
        mpu.setBW(MPU6050_BW_10);   //default set low pass filter bandwidth to 10HZ
        mpu.setGyroRange(MPU6050_GYRO_RANGE_500); //default set the gyro range to 500deg/s (robot turning exceeds 250deg/s)
        mpu.setAcceleroRange(MPU6050_ACCELERO_RANGE_2G);        //default set accelero range to 2g
        MPU_OK=1;
        myled = 0;  //turn on LED
        calibrate();
    } else if(0) { //this section is current disabled. change to (1) if you want to retry the accelerometer connection.
        myled = 1;  //turn off LED
        for (int i = 0; i<25; i++) {
            myled = !myled;
            wait_ms(50);
            if (mpu.testConnection()) {
                i = 25;
                myled = 0;
                MPU_OK=1;
            } else
                myled = 1;
            wait_ms(50);
        }
    }
    myled = MPU_OK;  //If LED is off, it is ok. If LED is on, there was a MPU error. Board needs to be restarted.
    //In the future, at a transistor to switch on/off the mpu in case there is an MPU error. Right now it needs a manual restart.

    bt.baud(BaudRate_bt);    //Set bluetooth baud rate

    //Initialize RF chip
//char txData[TRANSFER_SIZE], rxData[TRANSFER_SIZE];
// int txDataCnt = 0;
// int rxDataCnt = 0;
    rf24.powerUp();
    rf24.setTransferSize( TRANSFER_SIZE );

    rf24.setReceiveMode();
    rf24.enable();



}

//*********************************MOTORS*********************************//
void Robot::motor_control(int Lspeed, int Rspeed)
{
    //Controls the motors. 0 = stopped, 100 = full speed, -100 = reverse speed
    if (!Lspeed && !Rspeed)     //stop//
        STBY = 0;
    else
        STBY = 1;

    //make sure 'speeds' are between -100 and 100, and not between abs(0 to 10)
    if(Lspeed > 100) Lspeed = 100;
    else if (Lspeed < -100) Lspeed = -100;
    else if (Lspeed < 0 && Lspeed > -15)   Lspeed = -15;    //prevent speed from being less than 15
    else if (Lspeed > 0 &&  Lspeed < 15)    Lspeed = 15;
    if(Rspeed > 100) Rspeed = 100;
    else if (Rspeed < -100) Rspeed = -100;
    else if (Rspeed < 0 && Rspeed > -15)   Rspeed = -15;
    else if (Rspeed > 0 &&  Rspeed < 15)    Rspeed = 15;

    if (!Rspeed) {  //if right motor is stopped
        AIN1 = 0;
        AIN2 = 0;
        PWMA = 0;
    } else if (!Lspeed) { //if left motor is stopped
        BIN1 = 0;
        BIN2 = 0;
        PWMB = 0;
    }
    //RIGHT MOTOR//
    if(Rspeed > 0) {     //Right motor fwd
        AIN1 = MOTOR_R_DIRECTION;   //set the motor direction
        AIN2 = !AIN1;
    } else {     //Right motor reverse
        AIN2 = MOTOR_R_DIRECTION;
        AIN1 = !AIN2;
    }
    PWMA = abs(Rspeed/100.0);
    //LEFT MOTOR//
    if(Lspeed >0) {
        BIN1 = MOTOR_L_DIRECTION;
        BIN2 = !BIN1;
    } else {
        BIN2 = MOTOR_L_DIRECTION;
        BIN1 = !BIN2;
    }
    PWMB = abs(Lspeed/100.0);
}

void Robot::stop()
{
    motor_control(0,0);
}
void Robot::set_speed(int Speed)
{
    speed = Speed;
    motor_control(speed,speed);
}

//************************Setting direction of robot****************************//
void Robot::set_direction(double angle)
{
    //Set the direction the robot should be facing from origin(in radians)
    target_angle = angle;
}
void Robot::set_direction_deg(double angle)
{
    //Set the direction the robot should be facing from origin (in degrees)
    target_angle = angle*M_PI/180;
}
void Robot::auto_enable(bool x)
{
    AUTO_ORIENT = x;
}

//************************UPDATE: To be called in main loop***************************//
//This calculates position and angle
//Also orients robot in the correct direction
//if 'print' is 1, print acceleration and gyro data over bluetooth
void Robot::update()
{
    double SP;
    int rotationSpeed;
    //int n=50;

    double Kp = 100; //weighting values for PID controller. Only Proportional component is used.
    double Ki = 0;  //Integral component is not used
    double Kd = 0;  //derivative component is not used

    if(MPU_OK) { //only do this if MPU is connected
        wait(0.1);
        mpu.getAcceleroRaw(accdata);
        mpu.getGyroRaw(gyrodata);
        time = tt.read();
//bt.printf("s;t_%d;1_%lf;2_%lf;3_%d;4_%d;5_%d;6_%d;7_%d;8_%d;9_%lf;10_%lf;11_%lf;s\n\r",time_robot,getCurrent(),getVoltage(),accdata[0],accdata[1],accdata[2],gyrodata[0],gyrodata[1],gyrodata[2],irReadL(),irReadC(),irReadR());
        /*if(REMOTE_CONTROL) {    //if remote control over bluetooth is enabled
            m_ctrl=remote_ctrl();      //call the romote control function
        }*/
        if(AUTO_ORIENT) {       //enable PID control of angle
            SP = target_angle;  //desired angle (radians)
            rz = rz + ((gyrodata[2]-gyroOffset[2])*(time-timePrev)/gyroCorrect);    //rz is the rotation(radians) from start
            Irz = Irz + (rz-SP)*(time-timePrev);    //Irz needs to be reset every so often, or it should be ignored
            rotationSpeed = (Kp*(rz-SP) + Ki*Irz + Kd*gyrodata[2]/gyroCorrect);
            /*data[0]= getCurrent();
            data[1]= getVoltage();
            data[2]= accdata[0];
            data[3]= accdata[1];
            data[4]= accdata[2];
            data[5]= gyrodata[0];
            data[6]= gyrodata[1];
            data[7]= gyrodata[2];
            data[8]= irReadL();
            data[9]= irReadC();
            data[10]= irReadC();
            data[11]= rz;*/

            //TODO: pull "rotationspeed" up to 10 if it is less than 10. This will(should) improve the ability to drive straight

            if(time > timeNext || (speed==0 && rotationSpeed ==0)) {   //prevent the motor control from being set too often
                timeNext = time + MOTOR_INTERVAL/1000.0;   //only set the motor speed every 10ms
                //    if(abs(rz-SP)>(10*M_PI/180))
                //  {
                //  motor_control((speed+rotationSpeed)/20, (speed-rotationSpeed)/20);    //Set motor speeds
                //}
                // else
                //{
                motor_control((speed+rotationSpeed), (speed-rotationSpeed));    //Set motor speeds
                //}
            }
        }
        timePrev = time;
    }
}

void Robot::remote_ctrl(char c,int desired_speed,int desired_angle)
{
    //This is a private function
    Irz = 0;
    switch (c) {
        case ctrl_forward:
            speed = -1*desired_speed;
            break;
        case ctrl_backward:
            speed = 1*desired_speed;
            break;
        case ctrl_right:
            target_angle += 90*M_PI/180;
            break;
        case ctrl_left:
            target_angle -= 90*M_PI/180;
            break;
        case ctrl_calibrate:
            calibrate();
            break;
        case ctrl_turn_angle_cw:
            target_angle +=desired_angle*M_PI/180;
            break;
        case ctrl_turn_angle_ccw:
            target_angle -=desired_angle*M_PI/180;
            break;
        default:
            speed = 0;
            stop();
            break;
    }
}
//*********************************CALIBRATE*********************************//
void Robot::calibrate()
{
    stop();
    wait(1.5);
    double timeNOW = tt.read();
    int count=0;
    int i;

    //set the accelerometer and gyro offsets
    while(tt.read()<timeNOW+1.5) { //calculate gyro offset
        mpu.getGyroRaw(gyrodata);
        mpu.getAcceleroRaw(accdata);
        for(i=0; i<3; i++) {
            gyroOffset[i] += gyrodata[i];
            accOffset[i] += accdata[i];
        }
        count++;
    }
    for(i=0; i<3; i++) {
        gyroOffset[i] = gyroOffset[i]/count; //rxOffset
        accOffset[i] = accOffset[i]/count;
    }
    accOffset[2] = 0;   //we don't want to remove GRAVITY from the z-axis accelerometer.
}

//*********************************ROBOT-SENSORS*********************************//

double Robot::getCurrent()
{

    double Vsensor = currentSensor.read();
    Vsensor = 1000*((.8998-Vsensor)/.6411);
    return Vsensor;
}
double Robot::getVoltage()
{
    float voltage = 3.3*(voltageSensor.read()); // convert analog value to voltage at node
    voltage *= 1.5; // inverse of voltage divider
    return voltage;
}
void Robot::getIMU(float *adata, float *gdata)
{
    mpu.getAcceleroRaw(accdata);
    mpu.getGyroRaw(gyrodata);
    /*imuFilter.updateFilter(gyrodata[0],gyrodata[1],gyrodata[2],accdata[0],accdata[1],accdata[2]);
       imuFilter.computeEuler();
       gdata[0]=(float)imuFilter.getRoll();
       gdata[1]=(float)imuFilter.getPitch();
       gdata[2]= (float)imuFilter.getYaw();*/
    // adata[0] = (float) (accdata[0]);
    //adata[1] = (float) (accdata[1]);
    //adata[2] = (float) (accdata[2]);
    adata[0] = ((float) (accdata[0]-accOffset[0]))/32655*9.81*2;
    adata[1]= ((float) (accdata[1]-accOffset[1]))/32655*9.81*2;
    adata[2]= ((float) (accdata[2]-accOffset[2]))/32655*9.81*2;
    gdata[0]=(((float)(gyrodata[0]-gyroOffset[0]))/((float)gyroCorrect))*180/(M_PI);
    gdata[1]=(((float)(gyrodata[1]-gyroOffset[1]))/((float)gyroCorrect))*180/(M_PI);
    gdata[2]=(((float)(gyrodata[2]-gyroOffset[2]))/((float)gyroCorrect))*180/(M_PI);
}

double Robot::irReadL()
{
    int C = 14;
    double D = -1.1;
    double r = irSensorL.read();
    double reading=(pow(r,D)* C )+ 4;
    return reading;

}

double Robot::irReadC()
{
    int C = 14;
    double D = -1.1;
    double r = irSensorC.read();
    double reading=(pow(r,D)* C )+ 4;
    return reading;
}

double Robot::irReadR()
{
    int C = 14;
    double D = -1.1;
    double r = irSensorR.read();
    double reading=(pow(r,D)* C )+ 4;
    return reading;
}
double Robot::return_rotation()
{
    return rz;
}
int Robot::isMPU()
{
    return MPU_OK;
}

//******************RF24 CHIP FUNCTIONS****************************//
void Robot::rf24_power(int x)
{
    if(x)   //power up
        rf24.powerUp();
    else    //power down
        rf24.powerDown();
}