File content as of revision 3:89e4ed1324bb:

/*////////////////////////////////////////////////////////////////
ECE 4180 Final Project
Balancing Robot

Nico van Duijn
Samer Mabrouk
Anthony Agnone
Jay Danner
4/20/2015

This project consists of a robot balancing on two wheels. We use
the 9-axis IMU LSM9DS0 to give us Accelerometer and Gyroscope data
about the current angle and angular velocity of the robot. This
data is then filtered using complementary filters and PID control
to drive the two motors. The motors are controlled through digital
outputs in their direction and their seepd by PWM using an H-bridge.
The robot receives steering commands via the XBee module which is
interfaced with from a C# GUI that runs on a desktop computer.

*/////////////////////////////////////////////////////////////////

//////////////////////////////////////////////////////////////////
// Includes
#include "mbed.h"                                               // mbed library
#include "LSM9DS0.h"                                            // 9axis IMU
#include "math.h"                                               // We need to be able to do trig

//////////////////////////////////////////////////////////////////
// Constants
#define LSM9DS0_XM_ADDR  0x1D                                   // Would be 0x1E if SDO_XM is LOW
#define LSM9DS0_G_ADDR   0x6B                                   // Would be 0x6A if SDO_G is LOW
//#define DEBUG                                                  // Comment this out for final version

//////////////////////////////////////////////////////////////////
// I/O and object instatiation
PwmOut motorSpeedLeft(p21);                                     // PWM port to control speed of left motor
PwmOut motorSpeedRight(p22);                                    // PWM port to control speed of right motor
DigitalOut motorDirLeft(p24);                                   // Digital pin for direction of left motor
DigitalOut NmotorDirLeft(p23);                                  // Usually inverse of motorDirLeft
DigitalOut motorDirRight(p26);                                  // Digital pin for direction of right motor
DigitalOut NmotorDirRight(p25);                                 // Usually inverse of motorDirRight
LSM9DS0 imu(p9, p10, LSM9DS0_G_ADDR, LSM9DS0_XM_ADDR);          // Creates on object for IMU
Ticker start;                                                   // Initialize ticker to call control function
Ticker GUI;                                                     // Ticker that calls the updateGUI
DigitalOut led1(LED1);                                          // Use LED1 to provide some runtime info
Serial xbee(p13,p14);                                             // Create serial port for Xbee

typedef union _data {                                           // Typedef so we can jump from chars to floats
    float f;
    char chars[4];
    int i;
} myData;

//////////////////////////////////////////////////////////////////
// Globals // double battery, lab floor: 67/100/600/160 cutoff ..35
// positive turns
float kp = 59; //98                                                 // 145 Proportional gain kU 400-500
float kd = 105;  //200                                               // Derivative gain
float ki = 670;   //985                                             // Integrative gain
float OVERALL_SCALAR = 160;   //160                                  // Overall scalar that speed is divided by
float accBias = 0;                                              // Accelerometer Bias
float gyroBias = 0;                                             // Gyro Bias
float accAngle = 0;                                             // Global to hold angle measured by Accelerometer
float gyroAngle = 0;                                            // This variable holds the amount the angle has changed
float speed = 0;                                                // Variable for motor speed
float iAngle = 0;                                               // Integral value of angle-error (sum of gyro-angles)NOT EQUAL TO gyroAngle
float dAngle = 0;                                               // Derivative value for angle, angular velocity, how fast angle is changing
float pAngle = 0;                                               // Proportional value for angle, current angle (best measurement)
float desiredAngle=0;                                           // Setpoint. Set unequal zero to drive
float turnspeed=0;                                              // Makes the robot turn
myData bytes;                                                   // Used to convert received/sent chars to ints and floats


//////////////////////////////////////////////////////////////////
// Function Prototypes
void drive(float);                                              // Function declaration for driving the motors
void calibrate();                                               // Function to calibrate gyro and accelerometer
void control();                                                 // Function implementing PID control
void updateGUI();
void checkValues();

//////////////////////////////////////////////////////////////////
// Main function
int main()
{
    led1=0;                                                     // Turn led off
    uint16_t status = imu.begin();                              // Use the begin() function to initialize the LSM9DS0 library.
    xbee.baud(115200);                                            // Baudrate. Pretty high, check if lower possible
    calibrate();                                                // Calibrate gyro and accelerometer
    start.attach(&control, 0.01);                               // Looptime 10ms ca. 100Hz
    GUI.attach(&updateGUI, 0.5);                                // Update GUI twice a second

    while(1) {                                                  // Stay in this loop forever
        led1=!led1;                                             // Blink led1 to make sure the loop is running
        wait_ms(500);                                           // Looptime 500ms
    }
}

/////////////////////////////////////////////////////////////////
// Control function, implements PID
void control()
{
    dAngle=pAngle;                                              // remember old p-value
    imu.readGyro();                                             // Read the gyro
    imu.readAccel();                                            // Read the Accelerometer
    accAngle=(-1)*atan2(imu.ay,imu.az)*180/3.142-90-accBias;    // Like this, 0 is upright, forward is negative, backward positive
    gyroAngle=-(imu.gx-gyroBias)*0.01;                          // This is deltaangle, how much angle has changed
    pAngle=0.98*(pAngle+gyroAngle)+0.02*accAngle-desiredAngle;  // Complementary filter yields best value for current angle
    dAngle=pAngle-dAngle;                                       // Ang. Veloc. less noisy than dAngle = -(imu.gx-gyroBias);
    iAngle+=(pAngle*.01);                                       // integrate the angle (multiply by timestep to get dt!)

    if((abs(pAngle-desiredAngle)>=0.4)&&abs(pAngle-desiredAngle)<=15) {  // If it is tilted enough, but not too much ||abs(imu.gx)>10
        speed=-(ki*iAngle+kd*dAngle+kp*pAngle)/OVERALL_SCALAR;  // drive to correct
        if(speed<-1) speed=-1;                                  // Cap if undershoot
        else if(speed>1) speed=1;                               // Cap if overshoot
    } else speed=0;                                             // If we've fallen over or are steady on top
    drive(speed);                                               // Write speed to the motors
    checkValues();                                             // Checks if we need to update some values

}

//////////////////////////////////////////////////////////////////
// Drive function
void drive(float speed)
{
    int direction=0;                                            // Variable to hold direction we want to drive
    if (speed>0)direction=1;                                    // Positive speed indicates forward
    if (speed<0)direction=2;                                    // Negative speed indicates backwards
    if(speed==0)direction=0;                                    // Zero speed indicates stopping
    switch( direction) {                                        // Depending on what direction was passed
        case 0:                                                 // Stop case
            motorSpeedLeft=0;                                   // Set the DigitalOuts to stop the motors
            motorSpeedRight=0;
            motorDirLeft=0;
            NmotorDirLeft=0;
            motorDirRight=0;
            NmotorDirRight=0;
            break;
        case 1:                                                 // Forward case
            motorSpeedLeft=speed-turnspeed;                     // Set the DigitalOuts to run the motors forward
            motorSpeedRight=speed+turnspeed;
            motorDirLeft=1;
            NmotorDirLeft=0;
            motorDirRight=1;
            NmotorDirRight=0;
            break;
        case 2:                                                 // Backwards
            motorSpeedLeft=-speed+turnspeed;                    // Set the DigitalOuts to run the motors backward
            motorSpeedRight=-speed-turnspeed;
            motorDirLeft=0;
            NmotorDirLeft=1;
            motorDirRight=0;
            NmotorDirRight=1;
            break;
        default:                                                // Catch-all (Stop)
            motorSpeedLeft=0;                                   // Set the DigitalOuts to stop the motors
            motorSpeedRight=0;
            motorDirLeft=0;
            NmotorDirLeft=0;
            motorDirRight=0;
            NmotorDirRight=0;
            break;
    }
}

//////////////////////////////////////////////////////////////////
// Calibrate function to find gyro drift and accelerometer bias accbias: -0.3 gyrobias: +0.15
void calibrate()
{
    for(int i=0; i<100; i++) {                                  // Read a thousand values
        imu.readAccel();                                        // Read the Accelerometer
        imu.readGyro();                                         // Read the gyro
        accBias=accBias+(-1)*atan2(imu.ay,imu.az)*180/3.142-90; // Like this, 0 is upright, forward is negative, backward positive
        gyroBias=gyroBias+imu.gx;                               // Add up all the gyro Biases
    }
    accBias=accBias/100;                                        // Convert sum to average
    gyroBias=gyroBias/100;                                      // Convert sum to average
}


///////////////////////////////////////////////////////////////////
// updateGUI(), triggered by ticker GUI ever 0.5s, sends data to xbee
void updateGUI()
{
    xbee.putc('*');                                               // Send data validity value
    bytes.f = accBias;                                          // Send accBias
    xbee.printf("%c%c%c%c", bytes.chars[0], bytes.chars[1], bytes.chars[2], bytes.chars[3]);
    bytes.f = gyroBias;                                         // Send gyroBias
    xbee.printf("%c%c%c%c", bytes.chars[0], bytes.chars[1], bytes.chars[2], bytes.chars[3]);
    bytes.f = pAngle;                                           // Send P Angle
    xbee.printf("%c%c%c%c", bytes.chars[0], bytes.chars[1], bytes.chars[2], bytes.chars[3]);
    bytes.f = imu.gx;                                           // Send current angular velocity
    xbee.printf("%c%c%c%c", bytes.chars[0], bytes.chars[1], bytes.chars[2], bytes.chars[3]);
    bytes.f = turnspeed;                                        // Send turn speed
    xbee.printf("%c%c%c%c", bytes.chars[0], bytes.chars[1], bytes.chars[2], bytes.chars[3]);
    bytes.f = pAngle * kp;                                      // Send P Value
    xbee.printf("%c%c%c%c", bytes.chars[0], bytes.chars[1], bytes.chars[2], bytes.chars[3]);
    bytes.f = iAngle * ki;                                      // Send I Value
    xbee.printf("%c%c%c%c", bytes.chars[0], bytes.chars[1], bytes.chars[2], bytes.chars[3]);
    bytes.f = dAngle * kd;                                      // Send D Value
    xbee.printf("%c%c%c%c", bytes.chars[0], bytes.chars[1], bytes.chars[2], bytes.chars[3]);
    xbee.putc('\n');                                              // Send delimiting character
}
//////////////////////////////////////////////////////////////////
// checkValues() updates globals received from xbee
void checkValues()
{
    int i=0;                                                        // Integer needed for looping through input buffer
    char buffer[6];                                                 // Buffer to hold all the received data

    while(xbee.readable()) {                                          // As long as there is stuff in the buffer
        buffer[i++]=xbee.getc();                                      // Read from serial
    }

    if(buffer[0]== '*') {                                           // Check for 'start' character
        switch(buffer[1]) {                                         // Switch depending on what value we update
            case '1':                                               // Updating kp
                bytes.chars[0] = buffer[2];
                bytes.chars[1] = buffer[3];
                bytes.chars[2] = buffer[4];
                bytes.chars[3] = buffer[5];
                kp=bytes.f;
                break;
            case '2':                                               // Updating kd
                bytes.chars[0] = buffer[2];
                bytes.chars[1] = buffer[3];
                bytes.chars[2] = buffer[4];
                bytes.chars[3] = buffer[5];
                kd=bytes.f;
                break;
            case '3':                                               // Updating ki
                bytes.chars[0] = buffer[2];
                bytes.chars[1] = buffer[3];
                bytes.chars[2] = buffer[4];
                bytes.chars[3] = buffer[5];
                ki=bytes.f;
                break;
            case '4':                                                // Updating OVERALL_SCALAR
                bytes.chars[0] = buffer[2];
                bytes.chars[1] = buffer[3];
                bytes.chars[2] = buffer[4];
                bytes.chars[3] = buffer[5];
                OVERALL_SCALAR=bytes.f;
                break;
            case '5':                                                // Updating desiredAngle
                bytes.chars[0] = buffer[2];
                bytes.chars[1] = buffer[3];
                bytes.chars[2] = buffer[4];
                bytes.chars[3] = buffer[5];
                desiredAngle=bytes.f;
                break;
            case '6':                                               // Updating turnspeed
                bytes.chars[0] = buffer[2];
                bytes.chars[1] = buffer[3];
                bytes.chars[2] = buffer[4];
                bytes.chars[3] = buffer[5];
                turnspeed=bytes.f;
        }
    }

}