File content as of revision 19:9ea181b003af:

#include "mbed.h"
#include "rtos.h"
#include "LSM9DS1.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 angleBiasGyro = 0;
float angleBiasAcc = 0;


///////////////////////////////////////////////////////////////////////////////
///////////////////////////// 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);
    }
}


///////////////////////////////////////////////////////////////////////////////
///////////////////////////// Control System Updates///////////////////////////
///////////////////////////////////////////////////////////////////////////////
//make the calls to IMU here and this should be another thread
void update_system() {
    while(1){
        get_current_angle();
        //pc.printf("this is running 100");
        Thread::wait(10);
    }
}


///////////////////////////////////////////////////////////////////////////////
///////////////////////////// IMU STUFF////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
void calibrate_imu() {
    for(int i = 0; i < 500; i++){
        imu.readGyro();
        angleBiasGyro += imu.calcGyro(imu.gy);
        angleBiasAcc += (-1)*atan2(imu.ax,imu.az)*180/3.142-90
    }
    angleBiasGyro /= 500;
    angleBiasAcc /= 500;
}

void get_current_angle() {
//    return;
    imu.readGyro();
    int gyro = -(imu.calcGyro(imu.gy)-angleBiasGyro) * .01;
    int acc = (-1)*atan2(imu.ax,imu.az)*180/3.142-90-angleBiasAcc;
    //pc.printf("gyro:%f",gyro);
    angleMutex.lock();
    pAngle += 1*gyro //+ 0.02*acc;
    angleMutex.unlock();
}




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

int main() {
    pc.printf("this is running");
    calibrate_imu();
    Thread bluetooth;
    Thread system_update;
    
    imu.begin();
    
    bluetooth.start(bluetooth_update);
    system_update.start(update_system);
    
    //bluetooth.attach(&bluetooth_update, 0.1);
    while (1) {
        
        //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);
        angleMutex.unlock();
        
        
        //get_current_angle();
        
        
        Thread::wait(100);
    }
}