malcolm lear / Mbed 2 deprecated LabmbedV30

Labmbed device drivers

Dependencies:   TextLCD mbed

main.cpp@5:7eea83fb1cb4, 2017-01-27 (annotated)

Committer:
malcolmlear
Date:
Fri Jan 27 08:54:23 2017 +0000
Revision:
5:7eea83fb1cb4
Parent:
4:e2310d494d19
Child:
6:9ad19444c9ce
Tidied code and variables

Who changed what in which revision?

UserRevisionLine numberNew contents of line
malcolmlear 5:7eea83fb1cb4 1 // Device Drivers for Labmbed Board
malcolmlear 0:416329806e55 2
malcolmlear 0:416329806e55 3 #include "mbed.h"
malcolmlear 0:416329806e55 4 #include "TextLCD.h"
malcolmlear 0:416329806e55 5
malcolmlear 5:7eea83fb1cb4 6 TextLCD lcd(p15, p16, p17, p18, p19, p20); // LCD: RS, E, D4-D7
malcolmlear 5:7eea83fb1cb4 7 SPI spi(p5, p6, p7); // SPI: MOSI, MISO, SCLK (MISO not used with LCD)
malcolmlear 5:7eea83fb1cb4 8 DigitalOut lat(p8); // data latch for LED driver TLC59281
malcolmlear 5:7eea83fb1cb4 9 DigitalOut Sel0(p26); // input select bits:
malcolmlear 5:7eea83fb1cb4 10 DigitalOut Sel1(p25); // "
malcolmlear 5:7eea83fb1cb4 11 DigitalOut Sel2(p24); // "
malcolmlear 5:7eea83fb1cb4 12 DigitalIn In0(p14); // input from switches, keypad etc
malcolmlear 5:7eea83fb1cb4 13 DigitalIn In1(p13); // "
malcolmlear 5:7eea83fb1cb4 14 DigitalIn In2(p12); // "
malcolmlear 5:7eea83fb1cb4 15 DigitalIn In3(p11); // "
malcolmlear 5:7eea83fb1cb4 16 I2C i2c(p9, p10); // I2C: SDA, SCL
malcolmlear 0:416329806e55 17
malcolmlear 5:7eea83fb1cb4 18 // global variables
malcolmlear 5:7eea83fb1cb4 19 short LEDbits = 0; // global led status used for readback
malcolmlear 5:7eea83fb1cb4 20 const int TMP102Addr = 0x92; // TMP102 temperature I2C address
malcolmlear 5:7eea83fb1cb4 21 const int MPU6050Addr = 0xd0; // MPU-6050 accelerometer and Gyro I2C address
malcolmlear 5:7eea83fb1cb4 22 float Acceleration[3]; // MPU-6050 x,y,z acceleration values in 1G floating point
malcolmlear 5:7eea83fb1cb4 23 float GyroRate[3]; // MPU-6050 x,y,z gyrorates in degrees per second
malcolmlear 5:7eea83fb1cb4 24 float GyroOffset[3]; // MPU-6050 x,y,z gyrorates compensation
malcolmlear 5:7eea83fb1cb4 25 char AReg[] = { 0x3b, 0x3d, 0x3f }; // MPU-6050 I2C x,y,z accelerometer data registers
malcolmlear 5:7eea83fb1cb4 26 char GReg[] = { 0x43, 0x45, 0x47 }; // MPU-6050 I2C x,y,z gyro data registers
malcolmlear 0:416329806e55 27
malcolmlear 0:416329806e55 28
malcolmlear 5:7eea83fb1cb4 29 void InitLEDs() {
malcolmlear 5:7eea83fb1cb4 30 lat = 0; // latch must start low
malcolmlear 5:7eea83fb1cb4 31 spi.format(16,0); // SPI 16 bit data, low state, high going clock
malcolmlear 5:7eea83fb1cb4 32 spi.frequency(1000000); // 1MHz clock rate
malcolmlear 5:7eea83fb1cb4 33 }
malcolmlear 5:7eea83fb1cb4 34
malcolmlear 5:7eea83fb1cb4 35 void SetLEDs(short ledall) {
malcolmlear 5:7eea83fb1cb4 36 LEDbits = ledall; // update global led status
malcolmlear 5:7eea83fb1cb4 37 spi.write((LEDbits & 0x03ff) | ((LEDbits & 0xa800) >> 1) | ((LEDbits & 0x5400) << 1));
malcolmlear 5:7eea83fb1cb4 38 lat = 1; // latch pulse start
malcolmlear 5:7eea83fb1cb4 39 lat = 0; // latch pulse end
malcolmlear 0:416329806e55 40 }
malcolmlear 0:416329806e55 41
malcolmlear 5:7eea83fb1cb4 42 void SetLED(short LEDNo, short LEDState) {
malcolmlear 5:7eea83fb1cb4 43 LEDNo = ((LEDNo - 1) & 0x0007) + 1; // limit led number
malcolmlear 5:7eea83fb1cb4 44 LEDNo = (8 - LEDNo) * 2; // offset of led state in 'LEDbits'
malcolmlear 5:7eea83fb1cb4 45 LEDState = LEDState & 0x0003; // limit led state
malcolmlear 5:7eea83fb1cb4 46 LEDState = LEDState << LEDNo;
malcolmlear 5:7eea83fb1cb4 47 short statemask = ((0x0003 << LEDNo) ^ 0xffff); // mask used to clear led state
malcolmlear 5:7eea83fb1cb4 48 LEDbits = ((LEDbits & statemask) | LEDState); // clear and set led state
malcolmlear 5:7eea83fb1cb4 49 SetLEDs(LEDbits);
malcolmlear 5:7eea83fb1cb4 50 }
malcolmlear 5:7eea83fb1cb4 51
malcolmlear 5:7eea83fb1cb4 52 short ReadLED(short LEDNo) {
malcolmlear 5:7eea83fb1cb4 53 LEDNo = ((LEDNo - 1) & 0x0007) + 1; // limit led number
malcolmlear 5:7eea83fb1cb4 54 LEDNo = (8 - LEDNo) * 2; // offset of led state in 'LEDbits'
malcolmlear 5:7eea83fb1cb4 55 short LEDState = LEDbits;
malcolmlear 5:7eea83fb1cb4 56 LEDState = LEDState >> LEDNo; // shift selected led state into ls 2 bits
malcolmlear 5:7eea83fb1cb4 57 return (LEDState & 0x0003); // mask out and return led state
malcolmlear 5:7eea83fb1cb4 58 }
malcolmlear 5:7eea83fb1cb4 59
malcolmlear 5:7eea83fb1cb4 60 short ReadLEDs() {
malcolmlear 5:7eea83fb1cb4 61 return LEDbits; // return led status
malcolmlear 0:416329806e55 62 }
malcolmlear 0:416329806e55 63
malcolmlear 5:7eea83fb1cb4 64 void SelInput(short Input) {
malcolmlear 5:7eea83fb1cb4 65 Sel0 = Input & 0x0001; // set sel[0:2] pins
malcolmlear 5:7eea83fb1cb4 66 Sel1 = (Input >> 1) & 0x0001; //
malcolmlear 5:7eea83fb1cb4 67 Sel2 = (Input >> 2) & 0x0001; //
malcolmlear 5:7eea83fb1cb4 68 }
malcolmlear 5:7eea83fb1cb4 69
malcolmlear 5:7eea83fb1cb4 70 short ReadSwitches() {
malcolmlear 5:7eea83fb1cb4 71 SelInput(5); // select least significant 4 switches in[3:0]
malcolmlear 5:7eea83fb1cb4 72 short Switches = In0 + (In1 << 1) + (In2 << 2) + (In3 << 3);
malcolmlear 5:7eea83fb1cb4 73 SelInput(4); // select most significant 4 switches in[3:0]
malcolmlear 5:7eea83fb1cb4 74 return (Switches + (In0 << 4)+ (In1 << 5) + (In2 << 6) + (In3 << 7));
malcolmlear 0:416329806e55 75 }
malcolmlear 0:416329806e55 76
malcolmlear 5:7eea83fb1cb4 77 short ReadSwitch(short SwitchNo) {
malcolmlear 5:7eea83fb1cb4 78 SwitchNo = ((SwitchNo - 1) & 0x0007) + 1; // limit switch number
malcolmlear 5:7eea83fb1cb4 79 SwitchNo = 8 - SwitchNo; // offset of switch state in ReadSwitches()
malcolmlear 5:7eea83fb1cb4 80 short SwitchState = ReadSwitches(); // read switch states
malcolmlear 5:7eea83fb1cb4 81 SwitchState = SwitchState >> SwitchNo; // shift selected switch state into ls bit
malcolmlear 5:7eea83fb1cb4 82 return (SwitchState & 0x0001); // mask out and return switch state
malcolmlear 0:416329806e55 83 }
malcolmlear 0:416329806e55 84
malcolmlear 5:7eea83fb1cb4 85 short ReadKeys() {
malcolmlear 5:7eea83fb1cb4 86 SelInput(0); // select Keypad top row
malcolmlear 5:7eea83fb1cb4 87 short Keys = (In0 << 15) + (In1 << 14) + (In2 << 13) + (In3 << 12);
malcolmlear 5:7eea83fb1cb4 88 SelInput(1); // select Keypad second row
malcolmlear 5:7eea83fb1cb4 89 Keys = Keys + (In0 << 3) + (In1 << 6) + (In2 << 9) + (In3 << 11);
malcolmlear 5:7eea83fb1cb4 90 SelInput(2); // select Keypad third row
malcolmlear 5:7eea83fb1cb4 91 Keys = Keys + (In0 << 2) + (In1 << 5) + (In2 << 8) + In3;
malcolmlear 5:7eea83fb1cb4 92 SelInput(3); // select Keypad forth row
malcolmlear 5:7eea83fb1cb4 93 Keys = Keys + (In0 << 1) + (In1 << 4) + (In2 << 7) + (In3 << 10);
malcolmlear 5:7eea83fb1cb4 94 return (Keys ^ 0xffff); // return inverted (Key press active high)
malcolmlear 0:416329806e55 95 }
malcolmlear 0:416329806e55 96
malcolmlear 5:7eea83fb1cb4 97 short ReadKey(short KeyNo) {
malcolmlear 5:7eea83fb1cb4 98 KeyNo = KeyNo & 0x000f; // limit key number 0 to 15 (0 to F)
malcolmlear 5:7eea83fb1cb4 99 short KeyState = ReadKeys(); // read key states
malcolmlear 5:7eea83fb1cb4 100 KeyState = KeyState >> KeyNo; // shift selected key state into ls bit
malcolmlear 5:7eea83fb1cb4 101 return (KeyState & 0x0001); // mask out and return key state
malcolmlear 3:8eee79f59b30 102 }
malcolmlear 0:416329806e55 103
malcolmlear 5:7eea83fb1cb4 104 int FindKeyNo() {
malcolmlear 5:7eea83fb1cb4 105 short KeyNo;
malcolmlear 5:7eea83fb1cb4 106 short KeyPressed = -1; // set KeyPressed to -1 (no key pressed)
malcolmlear 5:7eea83fb1cb4 107 short KeyState = ReadKeys(); // read key states
malcolmlear 5:7eea83fb1cb4 108 for (KeyNo= 0; KeyNo < 16; KeyNo++ ) { // check all 16 Keys
malcolmlear 5:7eea83fb1cb4 109 if (KeyState & 0x0001) { // check key state
malcolmlear 5:7eea83fb1cb4 110 if (KeyPressed == -1) { // check if key already found
malcolmlear 5:7eea83fb1cb4 111 KeyPressed = KeyNo; // update KeyPressed
malcolmlear 5:7eea83fb1cb4 112 }
malcolmlear 5:7eea83fb1cb4 113 else {
malcolmlear 5:7eea83fb1cb4 114 return -1; // 2 or more keys pressed
malcolmlear 5:7eea83fb1cb4 115 }
malcolmlear 5:7eea83fb1cb4 116 }
malcolmlear 5:7eea83fb1cb4 117 KeyState = KeyState >> 1; // shift to check next key
malcolmlear 5:7eea83fb1cb4 118 }
malcolmlear 5:7eea83fb1cb4 119 return KeyPressed; // return KeyPressed
malcolmlear 3:8eee79f59b30 120 }
malcolmlear 0:416329806e55 121
malcolmlear 5:7eea83fb1cb4 122 char FindKeyChar() {
malcolmlear 5:7eea83fb1cb4 123 short KeyNo;
malcolmlear 5:7eea83fb1cb4 124 char KeyChar = ' '; // set KeyChar to ' ' (no key pressed)
malcolmlear 5:7eea83fb1cb4 125 KeyNo = FindKeyNo(); // find key pressed
malcolmlear 5:7eea83fb1cb4 126 if (KeyNo < 10 && KeyNo >= 0) {
malcolmlear 5:7eea83fb1cb4 127 KeyChar = (char) KeyNo + 0x30; // convert char 0-9 to ascii string '0'-'9'
malcolmlear 5:7eea83fb1cb4 128 }
malcolmlear 5:7eea83fb1cb4 129 if (KeyNo > 9 && KeyNo < 16) {
malcolmlear 5:7eea83fb1cb4 130 KeyChar = (char) KeyNo + 0x37; // convert char 10-15 to ascii string 'A'-'F'
malcolmlear 5:7eea83fb1cb4 131 }
malcolmlear 5:7eea83fb1cb4 132 return KeyChar; // return key pressed
malcolmlear 3:8eee79f59b30 133 }
malcolmlear 0:416329806e55 134
malcolmlear 5:7eea83fb1cb4 135 float ReadTemp() {
malcolmlear 5:7eea83fb1cb4 136 char Cmd[3];
malcolmlear 5:7eea83fb1cb4 137 Cmd[0] = 0x01; // pointer register value
malcolmlear 5:7eea83fb1cb4 138 Cmd[1] = 0x60; // byte 1 of the configuration register
malcolmlear 5:7eea83fb1cb4 139 Cmd[2] = 0xa0; // byte 2 of the configuration register
malcolmlear 5:7eea83fb1cb4 140 i2c.write(TMP102Addr, Cmd, 3); // select configuration register and write 0x60a0 to it
malcolmlear 5:7eea83fb1cb4 141 wait(0.5); // ensure conversion time
malcolmlear 5:7eea83fb1cb4 142 Cmd[0] = 0x00; // pointer register value
malcolmlear 5:7eea83fb1cb4 143 i2c.write(TMP102Addr, Cmd, 1); // select temperature register
malcolmlear 5:7eea83fb1cb4 144 i2c.read(TMP102Addr, Cmd, 2); // read 16-bit temperature register
malcolmlear 5:7eea83fb1cb4 145 return (float((Cmd[0] << 8) | Cmd[1]) / 256); // divide by 256 and return temperature
malcolmlear 5:7eea83fb1cb4 146 }
malcolmlear 5:7eea83fb1cb4 147
malcolmlear 5:7eea83fb1cb4 148 signed short ReadMPU6050(int RegAddr) {
malcolmlear 5:7eea83fb1cb4 149 char Cmd[3];
malcolmlear 5:7eea83fb1cb4 150 Cmd[0] = RegAddr; // register address
malcolmlear 5:7eea83fb1cb4 151 i2c.write(MPU6050Addr, Cmd, 1); // select register to read
malcolmlear 5:7eea83fb1cb4 152 i2c.read(MPU6050Addr, Cmd, 2); // read 2 bytes from register
malcolmlear 5:7eea83fb1cb4 153 return ((Cmd[0] << 8) | Cmd[1]); // return signed 16 bit value
malcolmlear 3:8eee79f59b30 154 }
malcolmlear 0:416329806e55 155
malcolmlear 5:7eea83fb1cb4 156 void CalibrateGyros() {
malcolmlear 5:7eea83fb1cb4 157 short a,b;
malcolmlear 5:7eea83fb1cb4 158 for(a=0; a<3; a++) {
malcolmlear 5:7eea83fb1cb4 159 GyroOffset[a] = 0; // clear gyro calibration offsets
malcolmlear 5:7eea83fb1cb4 160 for(b=0; b<1000; b++) {
malcolmlear 5:7eea83fb1cb4 161 GyroOffset[a] = GyroOffset[a] + (float)ReadMPU6050(GReg[a]);
malcolmlear 5:7eea83fb1cb4 162 wait_ms(1); // wait for next sample
malcolmlear 5:7eea83fb1cb4 163 }
malcolmlear 5:7eea83fb1cb4 164 GyroOffset[a] = GyroOffset[a]/1000; // find average over 1000 samples
malcolmlear 5:7eea83fb1cb4 165 }
malcolmlear 5:7eea83fb1cb4 166 }
malcolmlear 5:7eea83fb1cb4 167
malcolmlear 5:7eea83fb1cb4 168 void InitMotion() {
malcolmlear 5:7eea83fb1cb4 169 char Cmd[3];
malcolmlear 5:7eea83fb1cb4 170 Cmd[0] = 0xa1; // config register address
malcolmlear 5:7eea83fb1cb4 171 Cmd[1] = 0x06; // accelerometer and gyro bandwidth = 5Hz
malcolmlear 5:7eea83fb1cb4 172 i2c.write(MPU6050Addr, Cmd, 2); // write data to config register
malcolmlear 5:7eea83fb1cb4 173 Cmd[0] = 0x6b; // power management register address
malcolmlear 5:7eea83fb1cb4 174 Cmd[1] = 0x00; // data
malcolmlear 5:7eea83fb1cb4 175 i2c.write(MPU6050Addr, Cmd, 2); // write data to power management register
malcolmlear 5:7eea83fb1cb4 176 Cmd[0] = 0x1b; // gyro configuration register address
malcolmlear 5:7eea83fb1cb4 177 Cmd[1] = 0x08; // no gyro self test, +-500 full scale
malcolmlear 5:7eea83fb1cb4 178 i2c.write(MPU6050Addr, Cmd, 2); // write data to gyro configuration register
malcolmlear 5:7eea83fb1cb4 179 Cmd[0] = 0x19; // sample rate register address
malcolmlear 5:7eea83fb1cb4 180 Cmd[1] = 0x07; // sample rate = gyro output rate / 8
malcolmlear 5:7eea83fb1cb4 181 i2c.write(MPU6050Addr, Cmd, 2); // write data to sample rate register
malcolmlear 5:7eea83fb1cb4 182 CalibrateGyros();
malcolmlear 3:8eee79f59b30 183 }
malcolmlear 0:416329806e55 184
malcolmlear 5:7eea83fb1cb4 185 void ReadMotion() {
malcolmlear 5:7eea83fb1cb4 186 short a; // Acceleration is in G where 1G = 9.81 ms/s
malcolmlear 5:7eea83fb1cb4 187 for(a=0; a<3; a++) { // GyroRate is in degrees per second
malcolmlear 5:7eea83fb1cb4 188 Acceleration[a] = (float)ReadMPU6050(AReg[a]) / 16384;
malcolmlear 5:7eea83fb1cb4 189 GyroRate[a] = ((float)ReadMPU6050(GReg[a]) - GyroOffset[a]) / 66.5;
malcolmlear 5:7eea83fb1cb4 190 }
malcolmlear 5:7eea83fb1cb4 191 }
malcolmlear 0:416329806e55 192
malcolmlear 0:416329806e55 193 int main() {
malcolmlear 0:416329806e55 194
malcolmlear 5:7eea83fb1cb4 195 InitLEDs();
malcolmlear 5:7eea83fb1cb4 196 InitMotion();
malcolmlear 0:416329806e55 197
malcolmlear 0:416329806e55 198 while(1) {
malcolmlear 1:04e1ee8faa04 199 int a,b;
malcolmlear 5:7eea83fb1cb4 200 for (b = 0; b < 4; b++ ) { // select all 4 led states
malcolmlear 5:7eea83fb1cb4 201 for (a = 1; a < 9; a++ ) { // set all 8 leds to selected state
malcolmlear 5:7eea83fb1cb4 202 SetLED (a,b); // set led 'a' to state 'b'
malcolmlear 5:7eea83fb1cb4 203 wait(.05); // wait 0.05 second
malcolmlear 1:04e1ee8faa04 204 }
malcolmlear 1:04e1ee8faa04 205 }
malcolmlear 5:7eea83fb1cb4 206 for (a= 1; a < 9; a++ ) { // map Switch states to led's
malcolmlear 5:7eea83fb1cb4 207 SetLED (a,(ReadSwitch(a) + 1)); //
malcolmlear 5:7eea83fb1cb4 208 wait(.05); // wait 0.05 second
malcolmlear 3:8eee79f59b30 209 }
malcolmlear 5:7eea83fb1cb4 210 float temp = ReadTemp(); // get temperature
malcolmlear 5:7eea83fb1cb4 211 lcd.cls(); // clear lcd
malcolmlear 5:7eea83fb1cb4 212 lcd.printf("Temp = %f\n", temp); // print temperature
malcolmlear 5:7eea83fb1cb4 213 wait(1); // wait 1 second
malcolmlear 5:7eea83fb1cb4 214 lcd.cls(); // clear lcd
malcolmlear 5:7eea83fb1cb4 215 int swch = ReadSwitches(); // look at Switch states
malcolmlear 5:7eea83fb1cb4 216 lcd.printf("Switches = %d\n", swch); // print result
malcolmlear 5:7eea83fb1cb4 217 char Key = FindKeyChar(); // look for Key pressed
malcolmlear 5:7eea83fb1cb4 218 lcd.printf("Key = %c\n", Key); // print result
malcolmlear 5:7eea83fb1cb4 219 wait(1); // wait 1 second
malcolmlear 5:7eea83fb1cb4 220 ReadMotion(); // read new data in from the MPU-6050
malcolmlear 5:7eea83fb1cb4 221 lcd.cls(); // clear lcd
malcolmlear 5:7eea83fb1cb4 222 lcd.locate(0,0);
malcolmlear 5:7eea83fb1cb4 223 lcd.printf("x%.1f y%.1f z%.1f", Acceleration[0], Acceleration[1], Acceleration[2]);
malcolmlear 5:7eea83fb1cb4 224 lcd.locate(0,1);
malcolmlear 5:7eea83fb1cb4 225 lcd.printf("x%.1f y%.1f z%.1f", GyroRate[0], GyroRate[1], GyroRate[2]);
malcolmlear 5:7eea83fb1cb4 226 wait(.4);
malcolmlear 0:416329806e55 227 }
malcolmlear 0:416329806e55 228 }