File content as of revision 41:b9c8d527dd2b:

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


///////////////////////////////////////////////////////////////////////////////
///////////////////////////// Variable Initialization//////////////////////////
///////////////////////////////////////////////////////////////////////////////
Serial pc(USBTX, USBRX);                          // serial setup for debug


// Setup mutexes for the parameters and the angle values
Mutex parametersmutex;
Mutex angleMutex;


Serial blue(p28, p27);                             // setup bluetooth device

LSM9DS1 imu(p9, p10, 0xD6, 0x3C);                  // setup IMU  


///////////////////////////////////////////////////////////////////////////////
///////////////////////////// Control System Variables/////////////////////////
///////////////////////////////////////////////////////////////////////////////

// parameters for control system 
float rp = 50;                              // proportion parameter           
float rd = 60;                              // derivative parameter
float ri = 20;                              // integral parameter
float desired_angle = 0;                    // the angle the bot should lean at

volatile float speed = 0;                   // speed of the robot
volatile float turnSpeed = 0;               // turn speed when turning

float pAngle = 0;                           // angle error
float dAngle = 0;                           // change in angle error
float iAngle = 0;                           // integral of angle error


float angleBias = 0;                        // the angle bias set during calibration

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

bool isTurning = false;



///////////////////////////////////////////////////////////////////////////////
///////////////////////////// Defining Function ///////////////////////////////
///////////////////////////////////////////////////////////////////////////////
void get_current_angle();                   


///////////////////////////////////////////////////////////////////////////////
///////////////////////////// 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
                bhit = blue.getc(); //1=hit, 0=release
                parametersmutex.lock();        // set mutex since parameters are being updated
                if (blue.getc() == char(~('!' + 'B' + bnum + bhit))) { //checksum OK?
                    switch (bnum) {
                    case '1': //number button 1
                        if (bhit == '1') {
                            rd += 10;
                        } else {
                            //add release code here
                        }
                        break;
                    case '2': //number button 2
                        if (bhit == '1') {
                            ri += 10;
                        } else {
                            //add release code here
                        }
                        break;
                    case '3': //number button 3
                        if (bhit == '1') {
                            rd -= 10;
                        } else {
                            //add release code here
                        }
                        break;
                    case '4': //number button 4
                        if (bhit == '1') {
                            ri -= 10;
                        } else {
                            //add release code here
                        }
                        break;
                    case '5': //button 5 up arrow
                        if (bhit == '1') {
                            rp += 10;
                        } else {
                            //add release code here
                        }
                        break;
                    case '6': //button 6 down arrow
                        if (bhit == '1') {
                            rp -= 10;
                        } else {
                            //add release code here
                        }
                        break;
                    case '7': //button 7 left arrow
                        if (bhit == '1') {
//                            desired_angle -= 2;     //uncomment to change desired angle on press
                            turnSpeed = .3;           // comment to turn off turning
                            isTurning = !isTurning;
                        } else {
                            //add release code here
                        }
                        break;
                    case '8': //button 8 right arrow
                        if (bhit == '1') {
//                            desired_angle += 2;     //uncomment to change desired angle on press
                            turnSpeed = .3;           // comment to turn off turning
                            isTurning = !isTurning;
                        } else {
                            //add release code here
                        }
                        break;
                    default:
                        break;
                    }
                }
                parametersmutex.unlock();            // unlocking parameters mutex
            }
        }
        Thread::wait(100);                           // read bluetooth every tenth of a second
    }
}

///////////////////////////////////////////////////////////////////////////////
///////////////////////////// update motor speeds///////////////////////////
///////////////////////////////////////////////////////////////////////////////
void set_wheel_speed(float speed) {
    if (isTurning) {     // set bot to turning if enabled
        leftWheel.speed(turnSpeed*(1));
        rightWheel.speed(turnSpeed*(-1));
    } else {            // otherwise set the wheel speeds to the current speeds and max out at 1 (library requirements)
        if(speed > 1) speed = 1;
        if (speed < -1) speed = -1;
        leftWheel.speed(speed*(-1));
        rightWheel.speed(speed*(-1));
    }
}



///////////////////////////////////////////////////////////////////////////////
///////////////////////////// Control System Updates///////////////////////////
///////////////////////////////////////////////////////////////////////////////
//make the calls to IMU here and this should be another thread
void update_system() {
    while(1){
        get_current_angle();                       // get the current angle from function
        
        angleMutex.lock();                         // going to update angle values so lock mutex
        speed = -1*(rp*pAngle + ri*iAngle + rd*dAngle)/(70 * 150 * 3);     //set the speed based on angle error values
        set_wheel_speed(speed);                    // set the actual wheel speed
        angleMutex.unlock();                       // unlock the mutex
        
        Thread::wait(10);                          // update the sysetem every one hundreth of a second
    }
}


///////////////////////////////////////////////////////////////////////////////
///////////////////////////// IMU STUFF////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////

//used to find the angle bias
void calibrate_imu() {
    for(int i = 0; i < 500; i++){
        imu.readGyro();
        angleBias += imu.gy;
    }
    angleBias /= 500;
}



void get_current_angle() {
    imu.readGyro();                                 // read the gyro to get velocity of angle
    int gyro = -(imu.gy-angleBias) * .01;           // multiply by time to get the change in angle from before
    //pc.printf("gyro:%f",gyro);
    angleMutex.lock();                              // lock angle mutex 
    dAngle = pAngle;                                // save the old angle to be used later
    
    pAngle += 1*gyro; //+ 0.02*acc;                 //update the proportion error based on gyro 
    pAngle -= desired_angle*70;                     // get error based on the desired angle - 70 just random constant that worked well
    
    dAngle = pAngle - dAngle;                       // get the change in angle error
    
    iAngle += pAngle * 0.01;                        // update the integral of the angle error
    
    angleMutex.unlock();                            // unlock the mutex
}




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

int main() {

    // create the threads    
    Thread bluetooth;
    Thread system_update;
    
    // start and calibrate imu
    imu.begin();
    calibrate_imu();
    
    // start the threads
    bluetooth.start(bluetooth_update);
    system_update.start(update_system);
    
    while (1) {
        
        // print out values to debug
        angleMutex.lock();
        pc.printf("pAngle: %f", pAngle/70);
        pc.printf("    speed: %f", speed);
        parametersmutex.lock();
        pc.printf("rp: %f, rd: %f, ri: %f, desired_angle: %f\n\r", rp, rd, ri, desired_angle);
        parametersmutex.unlock();
        angleMutex.unlock();
        
        
        
        Thread::wait(100);  //print out 10 times a second
    }
}