main.cpp

00001 // Device Drivers for Labmbed Board
00002 
00003 #include "mbed.h"
00004 #include "TextLCD.h"
00005 
00006 TextLCD lcd(p15, p16, p17, p18, p19, p20);            // LCD: RS, E, D4-D7
00007 SPI spi(p5, p6, p7);                                  // SPI: MOSI, MISO, SCLK (MISO not used with LCD)
00008 DigitalOut lat(p8);                                   // data latch for LED driver TLC59281
00009 DigitalOut Sel0(p26);                                 // input select bits:
00010 DigitalOut Sel1(p25);                                 //  "
00011 DigitalOut Sel2(p24);                                 //  "
00012 DigitalIn In0(p14);                                   // input from switches, keypad etc
00013 DigitalIn In1(p13);                                   //  "
00014 DigitalIn In2(p12);                                   //  "
00015 DigitalIn In3(p11);                                   //  "
00016 I2C i2c(p9, p10);                                     // I2C: SDA, SCL
00017 
00018 // global variables
00019 short LEDbits = 0;                                    // global led status used for readback
00020 const int TMP102Addr = 0x92;                          // TMP102 temperature I2C address
00021 const int MPU6050Addr = 0xd0;                         // MPU-6050 accelerometer and Gyro I2C address
00022 float Acceleration[3];                                // MPU-6050 x,y,z acceleration values in 1G floating point
00023 float GyroRate[3];                                    // MPU-6050 x,y,z gyrorates in degrees per second
00024 float GyroOffset[3];                                  // MPU-6050 x,y,z gyrorates compensation
00025 char AReg[] = { 0x3b, 0x3d, 0x3f };                   // MPU-6050 I2C x,y,z accelerometer data registers
00026 char GReg[] = { 0x43, 0x45, 0x47 };                   // MPU-6050 I2C x,y,z gyro data registers
00027 
00028 
00029 void InitLEDs() {
00030     lat = 0;                                          // latch must start low
00031     spi.format(16,0);                                 // SPI 16 bit data, low state, high going clock
00032     spi.frequency(1000000);                           // 1MHz clock rate
00033 }
00034 
00035 void SetLEDs(short ledall) {
00036     LEDbits = ledall;                                 // update global led status
00037     spi.write((LEDbits & 0x03ff) | ((LEDbits & 0xa800) >> 1) | ((LEDbits & 0x5400) << 1));
00038     lat = 1;                                          // latch pulse start 
00039     lat = 0;                                          // latch pulse end
00040 }
00041 
00042 void SetLED(short LEDNo, short LEDState) {
00043     LEDNo = ((LEDNo - 1) & 0x0007) + 1;               // limit led number
00044     LEDState = LEDState & 0x0003;                     // limit led state
00045     LEDNo = (8 - LEDNo) * 2;                          // offset of led state in 'LEDbits'
00046     LEDState = LEDState << LEDNo;
00047     short statemask = ((0x0003 << LEDNo) ^ 0xffff);   // mask used to clear led state
00048     LEDbits = ((LEDbits & statemask) | LEDState);     // clear and set led state
00049     SetLEDs(LEDbits);
00050 }
00051 
00052 short ReadLED(short LEDNo) {
00053     LEDNo = ((LEDNo - 1) & 0x0007) + 1;               // limit led number
00054     LEDNo = (8 - LEDNo) * 2;                          // offset of led state in 'LEDbits'
00055     short LEDState = (LEDbits >> LEDNo) & 0x0003;     // shift selected led state into ls 2 bits
00056     return LEDState;                                  // return led state
00057 }
00058 
00059 short ReadLEDs() {
00060     return LEDbits;                                   // return led status
00061 }
00062 
00063 void SelInput(short Input) {
00064     Sel0 = Input & 0x0001;                            // set sel[0:2] pins
00065     Sel1 = (Input >> 1) & 0x0001;                     //
00066     Sel2 = (Input >> 2) & 0x0001;                     //
00067 }
00068 
00069 short ReadSwitches() {
00070     SelInput(5);                                      // select least significant 4 switches in[3:0]
00071     short Switches = In0 + (In1 << 1) + (In2 << 2) + (In3 << 3);
00072     SelInput(4);                                      // select most significant 4 switches in[3:0]
00073     return (Switches + (In0 << 4) + (In1 << 5) + (In2 << 6) + (In3 << 7));
00074 }
00075 
00076 short ReadSwitch(short SwitchNo) {
00077     SwitchNo = ((SwitchNo - 1) & 0x0007) + 1;         // limit switch number
00078     SwitchNo = 8 - SwitchNo;                          // offset of switch state in ReadSwitches()
00079     short SwitchState = ReadSwitches();               // read switch states
00080     SwitchState = SwitchState >> SwitchNo;            // shift selected switch state into ls bit
00081     return (SwitchState & 0x0001);                    // mask out and return switch state 
00082 }
00083 
00084 short ReadKeys() {
00085     SelInput(0);                                      // select Keypad top row 
00086     short Keys = (In0 << 15) + (In1 << 14) + (In2 << 13) + (In3 << 12);
00087     SelInput(1);                                      // select Keypad second row
00088     Keys += (In0 << 3) + (In1 << 6) + (In2 << 9) + (In3 << 11);   
00089     SelInput(2);                                      // select Keypad third row
00090     Keys += (In0 << 2) + (In1 << 5) + (In2 << 8) + In3;  
00091     SelInput(3);                                      // select Keypad forth row
00092     Keys += (In0 << 1) + (In1 << 4) + (In2 << 7) + (In3 << 10);
00093     return (Keys ^ 0xffff);                           // return inverted (Key press active high)
00094 }
00095 
00096 short ReadKey(short KeyNo) {
00097     KeyNo = KeyNo & 0x000f;                           // limit key number 0 to 15 (0 to F)
00098     short KeyState = ReadKeys();                      // read key states
00099     KeyState = KeyState >> KeyNo;                     // shift selected key state into ls bit
00100     return (KeyState & 0x0001);                       // mask out and return key state     
00101 }
00102 
00103 int FindKeyNo() {
00104     short KeyNo;
00105     short KeyPressed = -1;                            // set KeyPressed to -1 (no key pressed)
00106     short KeyState = ReadKeys();                      // read key states
00107     for (KeyNo= 0; KeyNo < 16; KeyNo++ ) {            // check all 16 Keys
00108         if (KeyState & 0x0001) {                      // check key state
00109             if (KeyPressed == -1) {                   // check if key already found
00110                 KeyPressed = KeyNo;                   // update KeyPressed
00111             }
00112             else {
00113                 return -1;                            // 2 or more keys pressed
00114             }
00115         }
00116         KeyState = KeyState >> 1;                     // shift to check next key
00117     }
00118     return KeyPressed;                                // return KeyPressed
00119 }
00120 
00121 char FindKeyChar() {
00122     short KeyNo;
00123     char KeyChar = ' ';                               // set KeyChar to ' ' (no key pressed)
00124     KeyNo = FindKeyNo();                              // find key pressed
00125     if (KeyNo < 10 && KeyNo >= 0) {
00126         KeyChar = (char) KeyNo + 0x30;                // convert char 0-9 to ascii string '0'-'9'
00127     }
00128     if (KeyNo > 9 && KeyNo < 16) {
00129         KeyChar = (char) KeyNo + 0x37;                // convert char 10-15 to ascii string 'A'-'F'
00130     }
00131     return KeyChar;                                   // return key pressed
00132 }
00133 
00134 float ReadTemp() {
00135     char Cmd[3];
00136     Cmd[0] = 0x01;                                    // pointer register value
00137     Cmd[1] = 0x60;                                    // byte 1 of the configuration register
00138     Cmd[2] = 0xa0;                                    // byte 2 of the configuration register
00139     i2c.write(TMP102Addr, Cmd, 3);                    // select configuration register and write 0x60a0 to it
00140     wait(0.5);                                        // ensure conversion time
00141     Cmd[0] = 0x00;                                    // pointer register value
00142     i2c.write(TMP102Addr, Cmd, 1);                    // select temperature register
00143     i2c.read(TMP102Addr, Cmd, 2);                     // read 16-bit temperature register 
00144     return (float((Cmd[0] << 8) | Cmd[1]) / 256);     // divide by 256 and return temperature
00145 }
00146 
00147 signed short ReadMPU6050(int RegAddr) {
00148     char Cmd[3];
00149     Cmd[0] = RegAddr;                                 // register address
00150     i2c.write(MPU6050Addr, Cmd, 1);                   // select register to read
00151     i2c.read(MPU6050Addr, Cmd, 2);                    // read 2 bytes from register
00152     return ((Cmd[0] << 8) | Cmd[1]);                  // return signed 16 bit value
00153 }
00154 
00155 void CalibrateGyros() {
00156     short a,b;
00157     for(a=0; a<3; a++) {
00158         GyroOffset[a] = 0;                            // clear gyro calibration offsets
00159         for(b=0; b<1000; b++) {
00160             GyroOffset[a] = GyroOffset[a] + (float)ReadMPU6050(GReg[a]);
00161             wait_ms(1);                               // wait for next sample
00162         }  
00163         GyroOffset[a] = GyroOffset[a]/1000;           // find average over 1000 samples
00164     }
00165 }
00166    
00167 void InitMotion() {
00168     char Cmd[3];
00169     Cmd[0] = 0xa1;                                    // config register address
00170     Cmd[1] = 0x06;                                    // accelerometer and gyro bandwidth = 5Hz
00171     i2c.write(MPU6050Addr, Cmd, 2);                   // write data to config register      
00172     Cmd[0] = 0x6b;                                    // power management register address
00173     Cmd[1] = 0x00;                                    // data
00174     i2c.write(MPU6050Addr, Cmd, 2);                   // write data to power management register   
00175     Cmd[0] = 0x1b;                                    // gyro configuration register address
00176     Cmd[1] = 0x08;                                    // no gyro self test, +-500 full scale
00177     i2c.write(MPU6050Addr, Cmd, 2);                   // write data to gyro configuration register
00178     Cmd[0] = 0x19;                                    // sample rate register address
00179     Cmd[1] = 0x07;                                    // sample rate = gyro output rate / 8
00180     i2c.write(MPU6050Addr, Cmd, 2);                   // write data to sample rate register    
00181     CalibrateGyros();           
00182 }
00183 
00184 void ReadMotion() {
00185     short a;                                          // Acceleration is in G where 1G = 9.81 ms/s
00186     for(a=0; a<3; a++) {                              // GyroRate is in degrees per second
00187         Acceleration[a] =  (float)ReadMPU6050(AReg[a]) / 16384;      
00188         GyroRate[a] = ((float)ReadMPU6050(GReg[a]) - GyroOffset[a]) / 66.5;
00189     }
00190 } 
00191 
00192 int main() {
00193     
00194     InitLEDs();
00195     InitMotion();
00196   
00197     while(1) {
00198         int a,b;
00199         for (b = 0; b < 4; b++ ) {                    // select all 4 led states
00200             for (a = 1; a < 9; a++ ) {                // set all 8 leds to selected state
00201                 SetLED (a,b);                         // set led 'a' to state 'b'
00202                 wait(.05);                            // wait 0.05 second
00203             }
00204         }
00205         for (a= 1; a < 9; a++ ) {                     // map Switch states to led's
00206             SetLED (a,(ReadSwitch(a) + 1));           //
00207             wait(.05);                                // wait 0.05 second
00208         }
00209         float temp = ReadTemp();                      // get temperature
00210         lcd.cls();                                    // clear lcd
00211         lcd.printf("Temp = %f\n", temp);              // print temperature
00212         wait(1);                                      // wait 1 second
00213         lcd.cls();                                    // clear lcd
00214         int swch = ReadSwitches();                    // look at Switch states   
00215         lcd.printf("Switches = %d\n", swch);          // print result
00216         char Key = FindKeyChar();                     // look for Key pressed
00217         lcd.printf("Key = %c\n", Key);                // print result
00218         wait(1);                                      // wait 1 second
00219         ReadMotion();                                 // read new data in from the MPU-6050
00220         lcd.cls();                                    // clear lcd
00221         lcd.locate(0,0);
00222         lcd.printf("x%.1f y%.1f z%.1f", Acceleration[0], Acceleration[1], Acceleration[2]);
00223         lcd.locate(0,1); 
00224         lcd.printf("x%.1f y%.1f z%.1f", GyroRate[0], GyroRate[1], GyroRate[2]);               
00225         wait(.4);
00226     }
00227 }