File content as of revision 0:59f6f94baeb1:

/** This program constantly sends the accelerometer data over the bluetooth to the computer.
*   The robot will drive FWD and REVERSE at maximum speed when you press W and S. //Turn 90 degrees at a time//
*   TODO: implement 90deg rotation 
*   TODO: Get rid of flags. Turn everything into a class
*   TODO: Use a PID controller for turning instead of whatever mess you used today
*/
#include "mbed.h"
#include "MPU6050.h"
#include "Terminal.h"
#include "string"
 #include "main.h"

//const int startMotor = 2;   //time (in seconds) when motor starts
//const int stopMotor = 4;    //time (in seconds) when motor stops 
 
Terminal pc(PTA2,PTA1);  //bluetooth

DigitalOut myled(PTE3);     //LED

MPU6050 mpu(PTE0, PTE1);    //Accelerometer (needs the MPU6050 library)

//things for the motor controller
PwmOut PWMA(PTA4);
PwmOut PWMB(PTA5);
DigitalOut AIN1(PTA14);
DigitalOut AIN2(PTA13);
DigitalOut BIN1(PTA16);
DigitalOut BIN2(PTA17);
DigitalOut STBY(PTA15);

//motor functions
void motor_control(bool motor, int speed, bool dir);    //speed should be between 1 and 100
void stop();
void straight(int speed);   //drive robot straight. Speed between -100 and 100. Accelerometer works best at 100
void turn_left(int speedA, int speedB);
void turn_right(int speedA, int speedB);

double integrate(data *prev, int acc[3], int time);     //integrate. this is not implemented yet
void remote_control(char c, int *turn);                      //Send a cmmand to this function, it will drive the robot

int accFlag = 0;    //flag to continue acceleration or deceleration (1 for acc, 0 for none, -1 for dec)
int turnFlag = 0;   //flag for direction of turn. If 0, no turn, if 1 turn right, -1 turn left
int accLimit = 16384.0/9.81* 1;  //acceleration limit = 2 m/s2
int maxRot = 0;


int main()
{
    double t2 = 0;
    double tnow;
    double SP;
    int Kp = 75; //proportional gain (tuning)
    int Ki = 0; //integral gain (tuning)
    double Kd = 0; //derivative gain (tuning) *Not used = 0*
    double Irz = 0;
    
    double rotationSpeed=0;
    double rz=0; //rotation in the z axis
    double tprev=0;//time of previous iteration
    double pi = 3.14159265359;
    int i=0;
    int turnTarget = 0; //angle for the robot to drive at (in degrees)
//    data *state;    //this will be passed into the integrate fucntion for recursiveness
    //send data to the integrate function, delayed by 10 samples
    //
    char c;
//    int flag = 0;
    int accdata[3];
    int avAcc[3];
    int gyrodata[3];
    Timer t;
    pc.baud(115200);    //Make sure this baud rate matches the baud rate of the bluetooth (set baud rate with "BluetoothAT")
    wait(0.1);
    char test = mpu.testConnection();


    pc.baud(115200); //this can only be changed after the BLUETOOTH baud rate has been changed
    if (mpu.testConnection()){
        //pc.printf("MPU connection succeeded\n\r");
        myled = 0;
        }
    else{
        pc.printf("MPU connection failed\n\r"); //Todo: If connection fails, retry a couple times. Try resetting MPU (this would need another wire?)
        myled = 1;
    }
    wait(3);

    t.reset();
  //  mpu.setBW(MPU6050_BW_10);   //set bandwidth of low pass filter for Accerlerometer AND Gyro.
  //  mpu.setBW(MPU6050_BW_42);    
 //    mpu.setBW(MPU6050_BW_98);    
    mpu.setBW(MPU6050_BW_20);
  //  mpu.setGyroRange(MPU6050_GYRO_RANGE_250);   //To convert gyro output to RAD/S, divide by 7505.7
    mpu.setGyroRange(MPU6050_GYRO_RANGE_500);   //to convert gyro output to RAD/S, divide by 3752.9
    //double gyroCorrect = 3752.9;    //////change this number!!!!Decrease to decrease rotation
    double gyroCorrect = 3720;   //prev3754.82prev3752.9  Use this number to convert gyro reading to rad/s
    int count = 0;
    double rzOff = 0;
    while(t.read()<1) //calculate gyro offset
    {
        mpu.getGyroRaw(gyrodata);
        rzOff += gyrodata[2];
        count++;
    }
    rzOff = rzOff/count; //rzOffset
    while(1) 
    {
        //speed = 100;
        mpu.getAcceleroRaw(accdata);
        mpu.getGyroRaw(gyrodata);
        
        //ROTATION//------------------------------------//
        
        //MV = 0; //input (speed setting)
        SP = turnTarget*pi/180; //desired angle (radians)
        tnow = t.read();
        rz = rz + ((gyrodata[2]-rzOff)*(tnow-tprev)/gyroCorrect);    //rz is the rotation(radians) from start
        Irz = Irz + (rz-SP)*(tnow-tprev); //Irz needs to be reset every so often, or it should be ignored
        tprev = tnow;
        rotationSpeed = (int(Kp*(rz-SP) + Ki*Irz + Kd*gyrodata[2]/gyroCorrect));
        pc.printf("\n\r%f\t",rotationSpeed);
        if(maxRot < gyrodata[2])
            maxRot = gyrodata[2];
            
            
        
        

        
        if(abs(rotationSpeed)<1)//(abs(rz-SP)<(0.0175*gyroCorrect))
        {
            rotationSpeed = 0;
            Irz = 0;
        }
        else if(abs(rotationSpeed)<10)
        {
            
            if (rotationSpeed>0)
                rotationSpeed = 10;
            else
                rotationSpeed = -10;
        }
        if(tnow>t2)
        {
            turn_right(-rotationSpeed,rotationSpeed);
            t2 = tnow+0.01;  
        }
        
        
        //END ROTATION//---------------------------------//
        if (accFlag > 0)
            accFlag --;
        else if(accFlag < 0)
            accFlag++;
     
     //  avAcc[0] = 0;
     //   avAcc[1] = 0;
     //   avAcc[2] = 0;
     //   for(i = 0; i<4; i++)
     //   {
     //   
     //       mpu.getAcceleroRaw(accdata);
     //       mpu.getGyroRaw(gyrodata);
     //       
     //       avAcc[0] += accdata[0];
    //        avAcc[1] += accdata[1];
    //        avAcc[2] += accdata[2];
    //    }

        
      pc.printf("%f\t%d\t%d\t%d\t", tnow,accdata[0], accdata[1], accdata[2]);
      // pc.printf("%d\t%d\t%d\t%d\n\r", gyrodata[0], gyrodata[1], gyrodata[2],STBY.read());
//      pc.printf("%f\n\r",tnow);
        
        //Old Code for moving the robot//
        /*
        if (t.read() > startMotor && flag ==0){
            myled = 0;
            straight(100);
            flag = 1;
        }
        if (t.read() > stopMotor){
            stop();
            myled = 1;
            STBY=0;
            flag = 2;
        }
        if (t.read() > 20)
            break;
            */
        //New code for moving robot//
        if(pc.readable() || accFlag){
            if(accFlag)
                c = 'i';
            else          
                c = pc.getc();
            remote_control(c,&turnTarget);
            if (c=='t'){        //reset timer if t is pressed
                wait(2);
                t.reset();
                turnTarget = 0;
            }
        }    
    }
        
}
void motor_control(bool motor, int speed, bool dir)     //motor: 0=right, 1=left;   speed: 0-100; dir 0=fwd, 1=bkwd                                 
{
    if (speed == 0)     //stop//
    {
        STBY = 0;
        if (motor ==0)
        {
            AIN1 = 0;
            AIN2 = 0;
            PWMA = 0;
        }
        else
        {
            BIN1 = 0;
            BIN2 = 0;
            PWMB = 0;
        }
    }
    else
        STBY = 1;
    if (!dir)    //forward//
    {
        if(motor == 0) //right motor//
        {
            AIN1 = 1;  
            AIN2 = 0;
            PWMA = abs(speed/100.0);
        }
        else            //left motor//
        {
            BIN1 = 0;  //REVERSE THESE WHEN FIX WIRING//////*****//////
            BIN2 = 1; 
            PWMB = abs(speed/100.0);
        }
    }
    else
    {
        if(motor == 0)  //right motor//
        {
            AIN1 = 0;
            AIN2 = 1;
            PWMA = abs(speed/100.0);
        }
        else            //left motor//
        {
            BIN1 = 1;   //REVERSE THESE WHEN FIXED WIRING!!////*****////////
            BIN2 = 0;
            PWMB = abs(speed/100.0);
        }
    }
    
}
void stop()
{
    motor_control(0,0,0);   //motorA, speed=0, direction = fwd
    motor_control(1,0,0);
    STBY=0;
}
void straight(int speed)    //if speed is +ve, go straight. -ve reverse
{
    if (speed == 0)
        stop();
    else if (speed > 100)
        speed = 100;
    else if (speed < -100)
        speed = -100;
    else if (speed>0)   //fwd//
    {
        motor_control(0,speed,0);
        motor_control(1,speed,0);
    }
    else
    {
        motor_control(0,speed,1);
        motor_control(1,speed,1);
    }
}
double integrate(int *prev, int acc[3], int time)
{
    /*Returns the distance travelled*/
    return 0;
}


/*Makes robot go straight, stop, and turn*/
void remote_control(char c, int *turn)
{
int speed;
int y;
    switch (c)
    {
        case 'w':   //forward - increment speed
 //           turn=0;
            speed = 100;
            straight(speed);
            break;
        case 's':   //backwards
//            turn=0;
            speed = -100;
            straight(speed);
            break;
        case 'd':   //right
            /*if(BIN1.read()==0 && AIN1.read()==0)     //if previously turning right, go straight
                stop();
            //{
            //    turn = 0;
            //    turnRate = 0;
            //    straight(speed);
            //}
            else
            {
                //turn_left(50,-50);
            }*/
            *turn+=90;    //rotate right 90 degrees
            break;
        case 'a':   //left
            /*if(BIN1.read() && AIN1.read())   //if previously turning left, go straight;
                stop();
            else
                turn_right(-50,50);
            */
            *turn-=90;    //rotate left 90 degrees
            break;
        case 'i':   //constant acceleration
            //if(BIN1.read() || (AIN!.read() == 0))   //if previously reverse, stop
            //    stop();
            //else    //go in reverse, at a constant acceleration
            speed = (PWMA.read() + PWMB.read())/2*100;
            y = mpu.getAcceleroRawY();
            if(speed<100 || BIN1.read() || (AIN1.read()==0) && accFlag < 2)
            {
                accFlag = 5;
                if(BIN1.read()) stop();
                if(abs(y)<accLimit)
                {
                    speed = speed + 1;
                    straight(speed);
                }
            }
            else
                accFlag = 0;
            break;
        default:    //stop
            speed = 0;
            stop();
    }
}


void turn_left(int speedA, int speedB)
{
    
    if (speedA < 0) //R reverse//
    {
        motor_control(0,speedA,1);  //Right reverse
        motor_control(1,speedB,1);  //Left reverse
    }
    else if(speedB <= 0)
    {
        motor_control(1,speedB,1);  //Left reverse//
        motor_control(0,speedA,0);  //Right fwd
    }
    else
    {
        motor_control(0,speedA,0);  //Right fwd
        motor_control(1,speedB,0);  //Left fwd
    }  
}
void turn_right(int speedA, int speedB)
{
    if (speedA > 0) //R fwd//
    {
        motor_control(0,speedA,0);  //Right fwd
    }
    else
    {
        motor_control(0,speedA,1);  //Right reverrse
    }
    if(speedB<0)   //L rev
    {
        motor_control(1,speedB,1);  //Left reverse
    }
    else
         motor_control(1,speedB,0); 
    
}