File content as of revision 3:dd0c62b586ea:

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

Nico van Duijn
Samer Mabrouk
Anthony Agnone
Jay 
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
#ifdef DEBUG
Serial pc(USBTX, USBRX);                                        // Creates virtual Serial connection though USB
#endif

//////////////////////////////////////////////////////////////////
// Globals
float kp = 98;   //40                                              // 145 Proportional gain kU 400-500
float kd = 200;   //2 was quite good                              // Derivative gain
float ki = 935;    //30                                              // Integrative gain
float OVERALL_SCALAR = 170;                                    // 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;

//////////////////////////////////////////////////////////////////
// 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
#ifdef DEBUG
void callback();                                                // Interrupt triggered function for serial communication
#endif

//////////////////////////////////////////////////////////////////
// Main function
int main()
{
    uint16_t status = imu.begin();                              // Use the begin() function to initialize the LSM9DS0 library.
#ifdef DEBUG
    pc.printf("LSM9DS0 WHO_AM_I's returned: 0x%X\n", status);   // Make sure communication is working
    pc.printf("Should be 0x49D4\n\n");                          // Check if we're talking to the right guy
    pc.attach(&callback);                                       // Attach interrupt to serial communication
#endif
    calibrate();                                                // Calibrate gyro and accelerometer
    start.attach(&control, 0.01);                               // Looptime 10ms ca. 100Hz
    while(1) {                                                  // Stay in this loop forever
#ifdef DEBUG
        pc.printf("%f\n",speed);                                // Print to USB the speed
#endif
    }
}

/////////////////////////////////////////////////////////////////
// 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
    //iAngle=(0.9*iAngle+0.1*gyroAngle);     DOESNT ACTUALLY INTEGRATE ERROR      // Sorta- running average-integral thing
    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((ki*iAngle/OVERALL_SCALAR)>=3)iAngle=3*OVERALL_SCALAR/ki;// if ki dominates three-fold
    //if((ki*iAngle/OVERALL_SCALAR)<=-3)iAngle=-3*OVERALL_SCALAR/ki;//50 is an arbitrary cap - kind of worked
    // try angle dev. .55 and find value for imu.gx
    if(abs(pAngle-desiredAngle)>=0.6&&abs(pAngle-desiredAngle)<=15) {                    // If it is tilted enough, but not too much
        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;                               // Set the DigitalOuts to run the motors forward
            motorSpeedRight=speed;
            motorDirLeft=1;
            NmotorDirLeft=0;
            motorDirRight=1;
            NmotorDirRight=0;
            break;
        case 2:                                                 // Backwards
            motorSpeedLeft=-speed;                              // Set the DigitalOuts to run the motors backward
            motorSpeedRight=-speed;
            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
#ifdef DEBUG
    pc.printf("accBias: %f gyroBias: %f\n",accBias, gyroBias);  // Print biases to USB
#endif
}

/////////////////////////////////////////////////////////////////
// Callback function to change values/gains
#ifdef DEBUG
void callback()
{
    char val;                                                   // Needed for Serial communication (need to be global?)
    val = pc.getc();                                            // Reat the value from Serial
    pc.printf("%c\n", val);                                     // Print it back to the screen
    if( val =='p') {                                            // If character was a 'p'
        pc.printf("enter kp \n");                               // Adjust kp
        val = pc.getc();                                        // Wait for kp value
        if(val == 0x2b) {                                       // If character is a plus sign
            kp=kp+10;                                           // Increase kp
        } else if (val == 0x2d) {                               // If recieved character is the minus sign
            kp=kp-10;                                           // Decrease kp
        } else {
            kp = val - 48;                                      // Cast char to float
        }
        pc.printf(" kp = %f \n",kp);                            // Print current kp value to screen
    } else if( val == 'd') {                                    // Adjust kd
        pc.printf("enter kd \n");                               // Wait for kd
        val= pc.getc();                                         // Read value from serial
        if(val == '+') {                                        // If given plus sign
            kd++;                                               // Increase kd
        } else if (val == '-') {                                // If given negative sign
            kd--;                                               // Decrease kd
        } else {                                                // If given some other ascii (a number?)
            kd = val - 48;                                      // Set derivative gain
        }
        pc.printf(" kd = %f \n",kd);                            // Print kd back to screen
    } else if( val == 'i') {                                    // If given i - integral gain
        pc.printf("enter ki \n");                               // Prompt on screen to ask for ascii
        val= pc.getc();                                         // Get the input
        if(val == '+') {                                        // If given the plus sign
            ki++;                                               // Increase ki
        } else if (val == '-') {                                // If given the minus sign
            ki--;                                               // Decrease ki
        } else {                                                // If given some other ascii
            ki = val - 48;                                      // Set ki to that number
        }
        pc.printf(" ki = %f \n",ki);
    } else if( val == 'o') {
        pc.printf("enter OVERALL_SCALAR \n");
        val= pc.getc();
        if(val == '+') {
            OVERALL_SCALAR=OVERALL_SCALAR+1000;
        } else if (val == '-') {
            OVERALL_SCALAR=OVERALL_SCALAR-1000;
        } else {
            OVERALL_SCALAR=(val-48)*1000;;
        }
        pc.printf(" OVERALL_SCALAR = %f \n",OVERALL_SCALAR);
    }
}
#endif