File content as of revision 1:0ef8f077473e:

#include "mbed.h"                                       // mbed library
#include "LSM9DS0.h"                                    // 9axis IMU
#include "math.h"

#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

PwmOut motorSpeedLeft(p21);                             // PWM port to control speed of left motor
PwmOut motorSpeedRight(p22);                            // PWM port to control speed of right motor
DigitalOut motorDirLeft(p23);                           // Digital pin for direction of left motor
DigitalOut NmotorDirLeft(p24);                          // 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
Serial pc(USBTX, USBRX);                                // Creates virtual Serial connection though USB
DigitalOut debug(p20);                                  // Digital output Pin for debugging purposes
Timer t;                                                // Timer needed to integrate
float kp;                                               // Proportional gain
float kd;                                               // Derivative gain
float ki;                                               // Integrative gain 
char val;                                               // Needed for Serial communication (need to be global?)
int timer1=0;                                           // Variable for timer
int timer2=0;                                           // Variable 2 for timer
void drive(float);                                      // Function declaration for driving the motors
void callback();                                        // Interrupt triggered function for serial communication
float PID(float);                                       // Function to do PID control


int main()
{

    uint16_t status = imu.begin();                              // Use the begin() function to initialize the LSM9DS0 library.
    pc.printf("LSM9DS0 WHO_AM_I's returned: 0x%X\n", status);   // Make sure communication is working
    pc.printf("Should be 0x49D4\n\n");
    float alpha=0;                                              // Local to hold angle
   // float speed;                                              // Local to hold motor speed
    //float omega;                                              // Local to hold angular velocity
    float desiredAngle=0;                                       // Later set this angle with a setup function
    pc.attach(&callback);                                       // Attach interrupt to serial communication 
    t.start();                                                  // Start the timer
      
    while(1) {
        timer2=timer1;                                          // Remember the previous timer
        timer1=t.read();                                        // Read the current timer
        imu.readGyro();                                         // Read the gyro
        imu.readAccel();                                        // Read the Accelerometer
        alpha=0.98*(alpha+imu.gx*(timer1-timer2))+0.02*(asin(imu.az));  // Complementary filter
        drive(PID(alpha-desiredAngle));                                 // PID control on the error
        
        
        /*
        alpha=asin(imu.az)*180/3.141;
        omega=imu.gx;
        speed=(kp*alpha+(kd/10.0)*omega)/100;
        drive(speed);
        pc.printf("%f\n", speed);
        wait(.1);

        
        data[0]=imu.ax;                             // x value of acceleration
        data[1]=imu.ay;                             // y value of acceleration
        data[2]=imu.az;                             // z value of acceleration
        */
        // pc.printf("Accelerometer Data:\nx: %f\ny: %f\nz: %f\n\n",imu.ax,imu.ay,imu.az);
        /*
        data[3]=imu.gx;                             // x value of gyro
        data[4]=imu.gy;                             // y value of gyro
        data[5]=imu.gz;                             // z value of gyro
        */
        //pc.printf("Gyroscope Data:\nx: %f\ny: %f\nz: %f\n\n",imu.gx,imu.gy,imu.gz);


    }
}
////////////////////////////////////////////////////////
// drive function
void drive(float speed)
{
    int direction=0;
    if (speed>0)direction=1;
    if (speed<0)direction=2;
    if(speed==0)direction=0;
   // pc.printf("%f %d\n", speed, direction);
    switch( direction) {                                // depending on what direction was passed
        case 0:                                         // stop case
            motorSpeedLeft=0;
            motorSpeedRight=0;
            motorDirLeft=0;
            NmotorDirLeft=0;
            motorDirRight=0;
            NmotorDirRight=0;
            break;
        case 1:                                         // forward case
            motorSpeedLeft=speed;
            motorSpeedRight=speed;
            motorDirLeft=1;
            NmotorDirLeft=0;
            motorDirRight=1;
            NmotorDirRight=0;
            break;
        case 2:                                         // backwards
            motorSpeedLeft=-speed;
            motorSpeedRight=-speed;
            motorDirLeft=0;
            NmotorDirLeft=1;
            motorDirRight=0;
            NmotorDirRight=1;
            break;
        default:                                        // catch-all
            motorSpeedLeft=0;
            motorSpeedRight=0;
            motorDirLeft=0;
            NmotorDirLeft=0;
            motorDirRight=0;
            NmotorDirRight=0;
            break;
    }
}


void callback() {
    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 == '+')
        {
            kp++;
        }else if (val == '-')
        {
            kp--;
        }else
        {
            kp = val - 48;                  // Cast char to float
        }
        pc.printf(" kp = %f \n",kp);
    }
    else if( val == 'd')                    // Adjust kd
    {
        pc.printf("enter kd \n");           // Wait for kd
        val= pc.getc();
        if(val == '+')
        {
            kd++;
        }else if (val == '-')
        {
            kd--;
        }else
        {
            kd = val - 48;
        }
        pc.printf(" kd = %f \n",kd);
    }
    else if( val == 'i')
    {
        pc.printf("enter ki \n");
        val= pc.getc();
        if(val == '+')
        {
            ki++;
        }else if (val == '-')
        {
            ki--;
        }else
        {
            ki = val - 48;
        }
        pc.printf(" ki = %f \n",kd);
    }
   
}

float PID(float error){                                             // Function to calculate speed given the error
    static float previousError=0;                                     // Static to keep track of previously passed error
    float integratedError=0;                                          // Local to hold the integrated error
    float derivativeError=0;                                          // Local for the derivative
    float speed=0;                                                    // Local for the speed
    integratedError+=error*(timer1-timer2);                         // Integrate aka. summing up the error*dt
    derivativeError=(error-previousError)/(timer1-timer2);          // Differentiate aka dy/dt
    speed=ki*integratedError+kp*error+kd*derivativeError;           // PID control
    previousError=error;                                            // Remember error for next time
    return speed;                                                   // Return speed
    }