File content as of revision 30:b1718c4edcd7:

#include "mbed.h"
#include "rtos.h"
#include "LSM9DS1.h"
#include "Motor.h"


///////////////////////////////////////////////////////////////////////////////
///////////////////////////// Variable Initialization//////////////////////////
///////////////////////////////////////////////////////////////////////////////
Ticker bluetooth;
Serial pc(USBTX, USBRX);


Mutex parametersmutex;
Mutex angleMutex;
Serial blue(p28, p27);

LSM9DS1 imu(p9, p10, 0xD6, 0x3C);


///////////////////////////////////////////////////////////////////////////////
///////////////////////////// Control System Variables/////////////////////////
///////////////////////////////////////////////////////////////////////////////
float rp = 50;
float rd = 51;
float ri = 50;
float desired_angle = 0;

float speed = 0;

float pAngle = 0;
float dAngle = 0;
float iAngle = 0;

int time_segment = 5;                        //Update the speed every 5 milliseconds

void get_current_angle();

float angleBias = 0;

Motor leftWheel(p21, p24, p23); // pwm, fwd, rev leftWheel(p21);
Motor rightWheel(p22, p26, p25); // pwm, fwd, rev rightWheel(p22);


///////////////////////////////////////////////////////////////////////////////
///////////////////////////// Bluetooth Section ///////////////////////////////
///////////////////////////////////////////////////////////////////////////////
void bluetooth_update() {
    char bnum = 0;
    char bhit = 0;
    while (1) {
        if (blue.getc() == '!') {
            if (blue.getc() == 'B') { //button data packet
                bnum = blue.getc(); //button number
                //pc.printf("%d",bnum);
                bhit = blue.getc(); //1=hit, 0=release
                parametersmutex.lock();
                if (blue.getc() == char(~('!' + 'B' + bnum + bhit))) { //checksum OK?
                    switch (bnum) {
                    case '1': //number button 1
                        if (bhit == '1') {
                            rd += 1;
                        } else {
                            //add release code here
                        }
                        break;
                    case '2': //number button 2
                        if (bhit == '1') {
                            ri += 1;
                        } else {
                            //add release code here
                        }
                        break;
                    case '3': //number button 3
                        if (bhit == '1') {
                            rd -= 1;
                        } else {
                            //add release code here
                        }
                        break;
                    case '4': //number button 4
                        if (bhit == '1') {
                            ri -= 1;
                        } else {
                            //add release code here
                        }
                        break;
                    case '5': //button 5 up arrow
                        if (bhit == '1') {
                            rp += 1;
                        } else {
                            //add release code here
                        }
                        break;
                    case '6': //button 6 down arrow
                        if (bhit == '1') {
                            rp -= 1;
                        } else {
                            //add release code here
                        }
                        break;
                    case '7': //button 7 left arrow
                        if (bhit == '1') {
                            desired_angle -= 1;
                        } else {
                            //add release code here
                        }
                        break;
                    case '8': //button 8 right arrow
                        if (bhit == '1') {
                            desired_angle += 1;
                        } else {
                            //add release code here
                        }
                        break;
                    default:
                        break;
                    }
                }
                parametersmutex.unlock();
            }
        }
        Thread::wait(100);
    }
}

///////////////////////////////////////////////////////////////////////////////
///////////////////////////// update motor speeds///////////////////////////
///////////////////////////////////////////////////////////////////////////////
void set_wheel_speed(float speed) {
    if(speed > 1) speed = 1;
    if (speed < -1) speed = -1;
    leftWheel.speed(speed);
    rightWheel.speed(speed);
}



///////////////////////////////////////////////////////////////////////////////
///////////////////////////// Control System Updates///////////////////////////
///////////////////////////////////////////////////////////////////////////////
//make the calls to IMU here and this should be another thread
void update_system() {
    while(1){
        get_current_angle();
        
        angleMutex.lock();
        speed = -1*(rp*pAngle + ri*iAngle + rd*dAngle)/(70 * 150 * 3);
        set_wheel_speed(speed); 
        angleMutex.unlock();
        
        //pc.printf("this is running 100");
        Thread::wait(10);
    }
}


///////////////////////////////////////////////////////////////////////////////
///////////////////////////// IMU STUFF////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
void calibrate_imu() {
    for(int i = 0; i < 500; i++){
        imu.readGyro();
        angleBias += imu.gy;
    }
    angleBias /= 500;
}

void get_current_angle() {
//    return;
    imu.readGyro();
    int gyro = -(imu.gy-angleBias) * .01;
    //pc.printf("gyro:%f",gyro);
    angleMutex.lock();
    dAngle = pAngle;
    
    pAngle += 1*gyro; //+ 0.02*acc;
    pAngle -= desired_angle*70;
    
    dAngle = pAngle - dAngle;
    
    iAngle += pAngle * 0.01;
    
    angleMutex.unlock();
}




///////////////////////////////////////////////////////////////////////////////
///////////////////////////// Running Main Function////////////////////////////
///////////////////////////////////////////////////////////////////////////////

int main() {
    pc.printf("this is running");
    
    Thread bluetooth;
    Thread system_update;
    
    imu.begin();
    calibrate_imu();
    bluetooth.start(bluetooth_update);
    system_update.start(update_system);
    
    //bluetooth.attach(&bluetooth_update, 0.1);
    while (1) {
        leftWheel.speed(1);
        rightWheel.speed(1);
        Thread::wait(10000);
        //bluetooth_update();
        //parametersmutex.lock();
//        pc.printf("rp: %f, rd: %f, ri: %f, desired_angle: %f\n", rp, rd, ri, desired_angle);
//        parametersmutex.unlock();
        
        angleMutex.lock();
        pc.printf("pAngle: %f", pAngle/70);
        pc.printf("    speed: %f\n\r", speed);
        angleMutex.unlock();
        
        
        //get_current_angle();
        
        
        Thread::wait(100);
    }
}