Faiyaz Doctor / Mbed 2 deprecated CE713-V01_Test

updated test

Dependencies:   TextLCD mbed

Fork of CE713-V01 by malcolm lear

main.cpp@0:4bc9e88c2cff, 2017-10-30 (annotated)

Committer:
malcolmlear
Date:
Mon Oct 30 11:27:45 2017 +0000
Revision:
0:4bc9e88c2cff
Child:
1:76c4a55fbac4
CE713 Drivers and Test Program

Who changed what in which revision?

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