malcolm lear
Labmbed device drivers
Diff: main.cpp
- Revision:
- 5:7eea83fb1cb4
- Parent:
- 4:e2310d494d19
- Child:
- 6:9ad19444c9ce
diff -r e2310d494d19 -r 7eea83fb1cb4 main.cpp
--- a/main.cpp Thu Jan 12 16:41:19 2017 +0000
+++ b/main.cpp Fri Jan 27 08:54:23 2017 +0000
@@ -1,135 +1,228 @@
-// Demo Program for Labmbed Board
+// 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
+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
-int led_bits = 0; // global LED status used for readback
-const int tmp102addr = 0x92; // TMP102 temperature I2C address
+// 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 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 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 setleds(int ledall) {
- led_bits = ledall; // update global LED status
- spi.write((led_bits & 0x03ff) | ((led_bits & 0xa800) >> 1) | ((led_bits & 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
+ LEDNo = (8 - LEDNo) * 2; // offset of led state in 'LEDbits'
+ LEDState = LEDState & 0x0003; // limit led state
+ 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;
+ LEDState = LEDState >> LEDNo; // shift selected led state into ls 2 bits
+ return (LEDState & 0x0003); // mask out and return led state
+}
+
+short ReadLEDs() {
+ return LEDbits; // return led status
}
-void setled(int ledno, int ledstate) {
- ledno = ((ledno - 1) & 0x0007) + 1; // limit led number
- ledno = (8 - ledno) * 2; // offset of led state in 'led_bits'
- ledstate = ledstate & 0x0003; // limit led state
- ledstate = ledstate << ledno;
- int statemask = ((0x0003 << ledno) ^ 0xffff); // mask used to clear led state
- led_bits = ((led_bits & statemask) | ledstate); // clear and set led state
- setleds(led_bits);
+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));
}
-int readled(int ledno) {
- ledno = ((ledno - 1) & 0x0007) + 1; // limit led number
- ledno = (8 - ledno) * 2; // offset of led state in 'led_bits'
- int ledstate = led_bits;
- ledstate = ledstate >> ledno; // shift selected led state into ls 2 bits
- return (ledstate & 0x0003); // mask out and return led state
+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
}
-int readleds() {
- return led_bits; // return LED status
+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 = Keys + (In0 << 3) + (In1 << 6) + (In2 << 9) + (In3 << 11);
+ SelInput(2); // select Keypad third row
+ Keys = Keys + (In0 << 2) + (In1 << 5) + (In2 << 8) + In3;
+ SelInput(3); // select Keypad forth row
+ Keys = Keys + (In0 << 1) + (In1 << 4) + (In2 << 7) + (In3 << 10);
+ return (Keys ^ 0xffff); // return inverted (Key press active high)
}
-void selinput(int input) {
- sel0 = input & 0x0001; // set sel[0:2] pins
- sel1 = (input >> 1) & 0x0001; //
- sel2 = (input >> 2) & 0x0001; //
+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 readswitches() {
- selinput(5); // select least significant 4 switches in[3:0]
- int 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));
+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
}
-int readswitch(int switchno) {
- switchno = ((switchno - 1) & 0x0007) + 1; // limit switch number
- switchno = 8 - switchno; // offset of switch state in readswitches()
- int switchstate = readswitches();
- switchstate = switchstate >> switchno; // shift selected switch state into ls bit
- return (switchstate & 0x0001); // mask out and return switch state
+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
}
-int readkeys() {
- selinput(0); // select keypad top row
- int keys = (in0 << 15) + (in1 << 14) + (in2 << 13) + (in3 << 12);
- selinput(1); // select keypad second row
- keys = keys + (in0 << 3) + (in1 << 6) + (in2 << 9) + (in3 << 11);
- selinput(2); // select keypad third row
- keys = keys + (in0 << 2) + (in1 << 5) + (in2 << 8) + in3;
- selinput(3); // select keypad forth row
- keys = keys + (in0 << 1) + (in1 << 4) + (in2 << 7) + (in3 << 10);
- return (keys ^ 0xffff); // return inverted (key press active high)
+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
}
-int readkey(int keyno) {
- keyno = keyno & 0x000f; // limit key number 0 to 15 (0 to F)
- int keystate = readkeys();
- keystate = keystate >> keyno; // shift selected key state into ls bit
- return (keystate & 0x0001); // mask out and return key state
+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();
}
-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); // wait for conversion
- 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.0); // divide by 256 and return temperature
-}
+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() {
- initleds();
+ InitLEDs();
+ InitMotion();
while(1) {
int a,b;
- for (b = 0; b < 4; b++ ) {
- for (a = 1; a < 9; a++ ) {
- setled (a,b);
- wait(.1);
+ 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++ ) {
- setled (a,(readswitch(a) * 3));
+ 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();
- lcd.cls();
- lcd.printf("Tmp = %f\n", temp);
- wait(1);
- lcd.cls();
- int switches = readswitches();
- lcd.printf("Switches = %d\n", switches);
- int keys = readkeys();
- lcd.printf("Keys = %d\n", keys);
+ 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
+ ReadMotion(); // read new data in from the MPU-6050
+ lcd.cls(); // clear lcd
+ lcd.locate(0,0);
+ lcd.printf("x%.1f y%.1f z%.1f", Acceleration[0], Acceleration[1], Acceleration[2]);
+ lcd.locate(0,1);
+ lcd.printf("x%.1f y%.1f z%.1f", GyroRate[0], GyroRate[1], GyroRate[2]);
+ wait(.4);
}
}