File content as of revision 0:4bc9e88c2cff:

// Device Drivers for Labmbed Board

#include "mbed.h"
#include "TextLCD.h"

TextLCD lcd(p15, p16, p17, p18, p19, p20);            // LCD: RS, E, D4-D7
SPI spi(p5, p6, p7);                                  // SPI: MOSI, MISO, SCLK (MISO not used with LCD)
DigitalOut lat(p8);                                   // data latch for LED driver TLC59281
DigitalOut Sel0(p26);                                 // input select bits:
DigitalOut Sel1(p25);                                 //  "
DigitalOut Sel2(p24);                                 //  "
DigitalIn In0(p14);                                   // input from switches, keypad etc
DigitalIn In1(p13);                                   //  "
DigitalIn In2(p12);                                   //  "
DigitalIn In3(p11);                                   //  "
I2C i2c(p9, p10);                                     // I2C: SDA, SCL pins
PwmOut servo1(p21);                                   // Servo 1 PWM pin
PwmOut servo2(p22);                                   // Servo 2 PWM pin
DigitalOut Trigger(p28);                              // Ultrasonic rangefinder trigger pin
DigitalIn  Echo(p27);                                 // Ultrasonic rangefinder echo pin
Timer Sonar;                                          // Ultrasonic timer

// global variables
short LEDbits = 0;                                    // global led status used for readback
const int TMP102Addr = 0x92;                          // TMP102 temperature I2C address
const int MPU6050Addr = 0xd0;                         // MPU-6050 accelerometer and Gyro I2C address
float Acceleration[3];                                // MPU-6050 x,y,z acceleration values in 1G floating point
float GyroRate[3];                                    // MPU-6050 x,y,z gyrorates in degrees per second
float GyroOffset[3];                                  // MPU-6050 x,y,z gyrorates compensation
char AReg[] = { 0x3b, 0x3d, 0x3f };                   // MPU-6050 I2C x,y,z accelerometer data registers
char GReg[] = { 0x43, 0x45, 0x47 };                   // MPU-6050 I2C x,y,z gyro data registers

void Servo1(float s) {                                // range +-1
    s=s+1;
    if (s>=0 && s<=2) {
        servo1.pulsewidth(s/2000+0.001);
    }
}

void Servo2(float s) {                                // range +-1
    s=s+1;
    if (s>=0 && s<=2) {
        servo2.pulsewidth(s/2000+0.001);
    }
}

void InitServos() {
    servo1.period(0.02);
    servo2.period(0.02);
    Servo1(0);                                        // initiate servo 1 to centre position
    Servo2(0);                                        // initiate servo 1 to centre position
}

int ReadSonar() {
    Trigger = 1;                                      // set sonar trigger pulse high
    Sonar.reset();                                    // reset sonar timer
    wait_us(10.0);                                    // 10 us pulse
    Trigger = 0;                                      // set sonar trigger pulse low
    while (Echo == 0) {};                             // wait for echo high (8 cycles have been transmitted)
    Sonar.start();                                    // echo high so start timer
    while (Echo == 1) {};                             // wait for echo low
    Sonar.stop();                                     // echo low so stop timer
    return (Sonar.read_us()*10)/58;                   // read timer and scale to mm
}

void InitLEDs() {
    lat = 0;                                          // latch must start low
    spi.format(16,0);                                 // SPI 16 bit data, low state, high going clock
    spi.frequency(1000000);                           // 1MHz clock rate
}

void SetLEDs(short ledall) {
    LEDbits = ledall;                                 // update global led status
    spi.write((LEDbits & 0x03ff) | ((LEDbits & 0xa800) >> 1) | ((LEDbits & 0x5400) << 1));
    lat = 1;                                          // latch pulse start 
    lat = 0;                                          // latch pulse end
}

void SetLED(short LEDNo, short LEDState) {
    LEDNo = ((LEDNo - 1) & 0x0007) + 1;               // limit led number
    LEDState = LEDState & 0x0003;                     // limit led state
    LEDNo = (8 - LEDNo) * 2;                          // offset of led state in 'LEDbits'
    LEDState = LEDState << LEDNo;
    short statemask = ((0x0003 << LEDNo) ^ 0xffff);   // mask used to clear led state
    LEDbits = ((LEDbits & statemask) | LEDState);     // clear and set led state
    SetLEDs(LEDbits);
}

short ReadLED(short LEDNo) {
    LEDNo = ((LEDNo - 1) & 0x0007) + 1;               // limit led number
    LEDNo = (8 - LEDNo) * 2;                          // offset of led state in 'LEDbits'
    short LEDState = (LEDbits >> LEDNo) & 0x0003;     // shift selected led state into ls 2 bits
    return LEDState;                                  // return led state
}

short ReadLEDs() {
    return LEDbits;                                   // return led status
}

void SelInput(short Input) {
    Sel0 = Input & 0x0001;                            // set sel[0:2] pins
    Sel1 = (Input >> 1) & 0x0001;                     //
    Sel2 = (Input >> 2) & 0x0001;                     //
}

short ReadSwitches() {
    SelInput(5);                                      // select least significant 4 switches in[3:0]
    short Switches = In0 + (In1 << 1) + (In2 << 2) + (In3 << 3);
    SelInput(4);                                      // select most significant 4 switches in[3:0]
    return (Switches + (In0 << 4) + (In1 << 5) + (In2 << 6) + (In3 << 7));
}

short ReadSwitch(short SwitchNo) {
    SwitchNo = ((SwitchNo - 1) & 0x0007) + 1;         // limit switch number
    SwitchNo = 8 - SwitchNo;                          // offset of switch state in ReadSwitches()
    short SwitchState = ReadSwitches();               // read switch states
    SwitchState = SwitchState >> SwitchNo;            // shift selected switch state into ls bit
    return (SwitchState & 0x0001);                    // mask out and return switch state 
}

short ReadKeys() {
    SelInput(0);                                      // select Keypad top row 
    short Keys = (In0 << 15) + (In1 << 14) + (In2 << 13) + (In3 << 12);
    SelInput(1);                                      // select Keypad second row
    Keys += (In0 << 3) + (In1 << 6) + (In2 << 9) + (In3 << 11);   
    SelInput(2);                                      // select Keypad third row
    Keys += (In0 << 2) + (In1 << 5) + (In2 << 8) + In3;  
    SelInput(3);                                      // select Keypad forth row
    Keys += (In0 << 1) + (In1 << 4) + (In2 << 7) + (In3 << 10);
    return (Keys ^ 0xffff);                           // return inverted (Key press active high)
}

short ReadKey(short KeyNo) {
    KeyNo = KeyNo & 0x000f;                           // limit key number 0 to 15 (0 to F)
    short KeyState = ReadKeys();                      // read key states
    KeyState = KeyState >> KeyNo;                     // shift selected key state into ls bit
    return (KeyState & 0x0001);                       // mask out and return key state     
}

int FindKeyNo() {
    short KeyNo;
    short KeyPressed = -1;                            // set KeyPressed to -1 (no key pressed)
    short KeyState = ReadKeys();                      // read key states
    for (KeyNo= 0; KeyNo < 16; KeyNo++ ) {            // check all 16 Keys
        if (KeyState & 0x0001) {                      // check key state
            if (KeyPressed == -1) {                   // check if key already found
                KeyPressed = KeyNo;                   // update KeyPressed
            }
            else {
                return -1;                            // 2 or more keys pressed
            }
        }
        KeyState = KeyState >> 1;                     // shift to check next key
    }
    return KeyPressed;                                // return KeyPressed
}

char FindKeyChar() {
    short KeyNo;
    char KeyChar = ' ';                               // set KeyChar to ' ' (no key pressed)
    KeyNo = FindKeyNo();                              // find key pressed
    if (KeyNo < 10 && KeyNo >= 0) {
        KeyChar = (char) KeyNo + 0x30;                // convert char 0-9 to ascii string '0'-'9'
    }
    if (KeyNo > 9 && KeyNo < 16) {
        KeyChar = (char) KeyNo + 0x37;                // convert char 10-15 to ascii string 'A'-'F'
    }
    return KeyChar;                                   // return key pressed
}

float ReadTemp() {
    char Cmd[3];
    Cmd[0] = 0x01;                                    // pointer register value
    Cmd[1] = 0x60;                                    // byte 1 of the configuration register
    Cmd[2] = 0xa0;                                    // byte 2 of the configuration register
    i2c.write(TMP102Addr, Cmd, 3);                    // select configuration register and write 0x60a0 to it
    wait(0.5);                                        // ensure conversion time
    Cmd[0] = 0x00;                                    // pointer register value
    i2c.write(TMP102Addr, Cmd, 1);                    // select temperature register
    i2c.read(TMP102Addr, Cmd, 2);                     // read 16-bit temperature register 
    return (float((Cmd[0] << 8) | Cmd[1]) / 256);     // divide by 256 and return temperature
}

signed short ReadMPU6050(int RegAddr) {
    char Cmd[3];
    Cmd[0] = RegAddr;                                 // register address
    i2c.write(MPU6050Addr, Cmd, 1);                   // select register to read
    i2c.read(MPU6050Addr, Cmd, 2);                    // read 2 bytes from register
    return ((Cmd[0] << 8) | Cmd[1]);                  // return signed 16 bit value
}

void CalibrateGyros() {
    short a,b;
    for(a=0; a<3; a++) {
        GyroOffset[a] = 0;                            // clear gyro calibration offsets
        for(b=0; b<1000; b++) {
            GyroOffset[a] = GyroOffset[a] + (float)ReadMPU6050(GReg[a]);
            wait_ms(1);                               // wait for next sample
        }  
        GyroOffset[a] = GyroOffset[a]/1000;           // find average over 1000 samples
    }
}
   
void InitMotion() {
    char Cmd[3];
    Cmd[0] = 0xa1;                                    // config register address
    Cmd[1] = 0x06;                                    // accelerometer and gyro bandwidth = 5Hz
    i2c.write(MPU6050Addr, Cmd, 2);                   // write data to config register      
    Cmd[0] = 0x6b;                                    // power management register address
    Cmd[1] = 0x00;                                    // data
    i2c.write(MPU6050Addr, Cmd, 2);                   // write data to power management register   
    Cmd[0] = 0x1b;                                    // gyro configuration register address
    Cmd[1] = 0x08;                                    // no gyro self test, +-500 full scale
    i2c.write(MPU6050Addr, Cmd, 2);                   // write data to gyro configuration register
    Cmd[0] = 0x19;                                    // sample rate register address
    Cmd[1] = 0x07;                                    // sample rate = gyro output rate / 8
    i2c.write(MPU6050Addr, Cmd, 2);                   // write data to sample rate register    
    CalibrateGyros();           
}

void ReadMotion() {
    short a;                                          // Acceleration is in G where 1G = 9.81 ms/s
    for(a=0; a<3; a++) {                              // GyroRate is in degrees per second
        Acceleration[a] =  (float)ReadMPU6050(AReg[a]) / 16384;      
        GyroRate[a] = ((float)ReadMPU6050(GReg[a]) - GyroOffset[a]) / 66.5;
    }
} 

int main() {
    
    float spos = 0;                                   // Test servo position
    InitLEDs();
    InitMotion();
    InitServos();
  
    while(1) {
        int a,b;
        for (b = 0; b < 4; b++ ) {                    // select all 4 led states
            for (a = 1; a < 9; a++ ) {                // set all 8 leds to selected state
                SetLED (a,b);                         // set led 'a' to state 'b'
                wait(.05);                            // wait 0.05 second
            }
        }
        for (a= 1; a < 9; a++ ) {                     // map Switch states to led's
            SetLED (a,(ReadSwitch(a) + 1));           //
            wait(.05);                                // wait 0.05 second
        }
        float temp = ReadTemp();                      // get temperature
        lcd.cls();                                    // clear lcd
        lcd.printf("Temp = %f\n", temp);              // print temperature
        wait(1);                                      // wait 1 second
        lcd.cls();                                    // clear lcd
        int swch = ReadSwitches();                    // look at Switch states   
        lcd.printf("Switches = %d\n", swch);          // print result
        char Key = FindKeyChar();                     // look for Key pressed
        lcd.printf("Key = %c\n", Key);                // print result
        wait(1);                                      // wait 1 second
        lcd.cls();                                    // clear lcd
        int dist = ReadSonar();                       // get distance
        lcd.printf("Distance = %d\n", dist);          // print result
        lcd.printf("Servo = %f\n", spos);             // print servo pos
        wait(1);                                      // wait 1 second
        ReadMotion();                                 // read new data in from the MPU-6050
        lcd.cls();                                    // clear lcd
        lcd.locate(0,0);                              // print at start of first line
        lcd.printf("x%.1f y%.1f z%.1f", Acceleration[0], Acceleration[1], Acceleration[2]);
        lcd.locate(0,1);                              // print at start of second line
        lcd.printf("x%.1f y%.1f z%.1f", GyroRate[0], GyroRate[1], GyroRate[2]);               
        wait(.4);                                     // wait 0.4 second
        if (spos < 1) {                               // is servo at upper limit of 1
          spos += .1;                                 // increment servo position
        }                                             //
        else {                                        // was at upper limit so
          spos = -1;                                  // reset servo position
        }                                             //
        Servo1(spos);                                 // update servo
    }
}