File content as of revision 2:89ada0b0b923:

/*////////////////////////////////////////////////////////////////
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;
//Serial xbee(p13, p14);                                          // Creates virtual serial for xbee
Serial pc(USBTX,USBRX);// later only use xbee
Timer t; //debugging only
DigitalOut led1(LED1); // debug only
DigitalOut led2(LED2); // debug only
DigitalOut led3(LED3); // debug only
typedef union _data { // maybe there is a better way?
    float f;
    char chars[4];
    int i; // added to receive integers
} myData;


//////////////////////////////////////////////////////////////////
// Globals // double battery, lab floor: 67/100/600/160 cutoff ..35
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;                                              // positive turns
myData bytes;

//////////////////////////////////////////////////////////////////
// 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 callback();                                                // Interrupt triggered function for serial communication
void sendFloatAsBytes();
void updateGUI();
//////////////////////////////////////////////////////////////////
// Main function
int main()
{

    led1=0;//debug only
    uint16_t status = imu.begin();                              // Use the begin() function to initialize the LSM9DS0 library.
    pc.baud(115200);//pretty high, check if lower possible
    pc.attach(&callback);                                       // Attach interrupt to serial communication
    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;
        wait_ms(500); // blink twice a second
    }
}

/////////////////////////////////////////////////////////////////
// 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

}

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

/////////////////////////////////////////////////////////////////
// Callback function to change values/gains
void callback()
{
    led2=!led2; // Blink led2 so we know what's happening
    char buffer[24]; //create a buffer
    int i=0;
    while(pc.readable()) { // read the whole damn thing
        buffer[i]=pc.getc();
        i++;
    }
// update kp
    bytes.chars[0]=buffer[0];
    bytes.chars[1]=buffer[1];
    bytes.chars[2]=buffer[2];
    bytes.chars[3]=buffer[3];
    kp=bytes.i;
// update ki
    bytes.chars[0]=buffer[4];
    bytes.chars[1]=buffer[5];
    bytes.chars[2]=buffer[6];
    bytes.chars[3]=buffer[7];
    ki=bytes.i;
// update kd
    bytes.chars[0]=buffer[8];
    bytes.chars[1]=buffer[9];
    bytes.chars[2]=buffer[10];
    bytes.chars[3]=buffer[11];
    kd=bytes.i;
// update OVERALL_SCALAR
    bytes.chars[0]=buffer[12];
    bytes.chars[1]=buffer[13];
    bytes.chars[2]=buffer[14];
    bytes.chars[3]=buffer[15];
    OVERALL_SCALAR=bytes.i;
// update turnspeed
    bytes.chars[0]=buffer[16];
    bytes.chars[1]=buffer[17];
    bytes.chars[2]=buffer[18];
    bytes.chars[3]=buffer[19];
    turnspeed=bytes.i;
// update desiredAngle
    bytes.chars[0]=buffer[20];
    bytes.chars[1]=buffer[21];
    bytes.chars[2]=buffer[22];
    bytes.chars[3]=buffer[23];
    desiredAngle=bytes.i;


    // Update kp min: 0 max: 3000
    // Update kd min: 0 max: 3000
    // Update ki min: 0 max: 3000
    // Update OVERALL_SCALAR  min: 0 max: 3000
    // Update turnspeed min: -0.5 max: 0.5
    // Update desiredAngle min: -3 max: 3
    // Send pAngle BETTER: kp*pAngle
    // Send dAngle BETTER: kd*dAngle
    // Send iAngle BETTER: kd*dAngle
}
//////////////////////////////////////////////////////////////////
// maybe get rid of this function and put it into updateGUI to run faster
void sendFloatAsBytes()
{
    pc.printf("%c%c%c%c", bytes.chars[0], bytes.chars[1], bytes.chars[2], bytes.chars[3]);
}

///////////////////////////////////////////////////////////////////
// triggered by ticker GUI ever 0.5s, sends data to PC
void updateGUI()
{
    led3=!led3; // blink so we know what's going on
    pc.putc('6'); // 6 is for pTerm
    bytes.f=pAngle; // later kp*pAngle
    sendFloatAsBytes();
    wait_us(10); // waiting a little bit is key
    pc.putc('7');
    bytes.f=iAngle;
    sendFloatAsBytes();
    wait_us(10);
    pc.putc('8');
    bytes.f=dAngle;
    sendFloatAsBytes();
}