![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
updated test
Fork of CE713-V01 by
main.cpp@1:76c4a55fbac4, 2017-11-09 (annotated)
- Committer:
- Faiyaz
- Date:
- Thu Nov 09 10:09:30 2017 +0000
- Revision:
- 1:76c4a55fbac4
- Parent:
- 0:4bc9e88c2cff
test version
Who changed what in which revision?
User | Revision | Line number | New 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(); |
Faiyaz | 1:76c4a55fbac4 | 234 | InitServos(); |
Faiyaz | 1:76c4a55fbac4 | 235 | |
Faiyaz | 1:76c4a55fbac4 | 236 | float temp = -99; |
Faiyaz | 1:76c4a55fbac4 | 237 | int dist = -99; |
Faiyaz | 1:76c4a55fbac4 | 238 | char Key = 'X'; |
malcolmlear | 0:4bc9e88c2cff | 239 | |
malcolmlear | 0:4bc9e88c2cff | 240 | while(1) { |
Faiyaz | 1:76c4a55fbac4 | 241 | |
Faiyaz | 1:76c4a55fbac4 | 242 | |
malcolmlear | 0:4bc9e88c2cff | 243 | int a,b; |
Faiyaz | 1:76c4a55fbac4 | 244 | /* |
malcolmlear | 0:4bc9e88c2cff | 245 | for (b = 0; b < 4; b++ ) { // select all 4 led states |
malcolmlear | 0:4bc9e88c2cff | 246 | for (a = 1; a < 9; a++ ) { // set all 8 leds to selected state |
malcolmlear | 0:4bc9e88c2cff | 247 | SetLED (a,b); // set led 'a' to state 'b' |
malcolmlear | 0:4bc9e88c2cff | 248 | wait(.05); // wait 0.05 second |
malcolmlear | 0:4bc9e88c2cff | 249 | } |
malcolmlear | 0:4bc9e88c2cff | 250 | } |
Faiyaz | 1:76c4a55fbac4 | 251 | */ |
Faiyaz | 1:76c4a55fbac4 | 252 | |
Faiyaz | 1:76c4a55fbac4 | 253 | int firstOnSwitch = 0; |
Faiyaz | 1:76c4a55fbac4 | 254 | |
malcolmlear | 0:4bc9e88c2cff | 255 | for (a= 1; a < 9; a++ ) { // map Switch states to led's |
malcolmlear | 0:4bc9e88c2cff | 256 | SetLED (a,(ReadSwitch(a) + 1)); // |
Faiyaz | 1:76c4a55fbac4 | 257 | |
Faiyaz | 1:76c4a55fbac4 | 258 | if(ReadSwitch(a) == 1) |
Faiyaz | 1:76c4a55fbac4 | 259 | { |
Faiyaz | 1:76c4a55fbac4 | 260 | firstOnSwitch = a; |
Faiyaz | 1:76c4a55fbac4 | 261 | } |
Faiyaz | 1:76c4a55fbac4 | 262 | |
Faiyaz | 1:76c4a55fbac4 | 263 | wait(0.05); // wait 0.05 second |
malcolmlear | 0:4bc9e88c2cff | 264 | } |
Faiyaz | 1:76c4a55fbac4 | 265 | |
Faiyaz | 1:76c4a55fbac4 | 266 | switch(firstOnSwitch) |
Faiyaz | 1:76c4a55fbac4 | 267 | { |
Faiyaz | 1:76c4a55fbac4 | 268 | case 1: |
Faiyaz | 1:76c4a55fbac4 | 269 | temp = ReadTemp(); // get temperature |
Faiyaz | 1:76c4a55fbac4 | 270 | lcd.cls(); // clear lcd |
Faiyaz | 1:76c4a55fbac4 | 271 | lcd.printf("Temp = %f\n", temp); // print temperature |
Faiyaz | 1:76c4a55fbac4 | 272 | wait(1); // wait 1 second |
Faiyaz | 1:76c4a55fbac4 | 273 | break; |
Faiyaz | 1:76c4a55fbac4 | 274 | |
Faiyaz | 1:76c4a55fbac4 | 275 | case 2: |
Faiyaz | 1:76c4a55fbac4 | 276 | dist = ReadSonar(); // get distance |
Faiyaz | 1:76c4a55fbac4 | 277 | lcd.cls(); |
Faiyaz | 1:76c4a55fbac4 | 278 | lcd.printf("Distance = %d\n", dist); // print result |
Faiyaz | 1:76c4a55fbac4 | 279 | //lcd.printf("Servo = %f\n", spos); // print servo pos |
Faiyaz | 1:76c4a55fbac4 | 280 | wait(0.5); |
Faiyaz | 1:76c4a55fbac4 | 281 | break; |
Faiyaz | 1:76c4a55fbac4 | 282 | |
Faiyaz | 1:76c4a55fbac4 | 283 | case 3: |
Faiyaz | 1:76c4a55fbac4 | 284 | Key = FindKeyChar(); // look for Key pressed |
Faiyaz | 1:76c4a55fbac4 | 285 | lcd.cls(); |
Faiyaz | 1:76c4a55fbac4 | 286 | lcd.printf("Key = %c\n", Key); // print result |
Faiyaz | 1:76c4a55fbac4 | 287 | wait(0.5); // wait 1 second |
Faiyaz | 1:76c4a55fbac4 | 288 | break; |
Faiyaz | 1:76c4a55fbac4 | 289 | |
Faiyaz | 1:76c4a55fbac4 | 290 | // operator is doesn't match any case constant (+, -, *, /) |
Faiyaz | 1:76c4a55fbac4 | 291 | default: |
Faiyaz | 1:76c4a55fbac4 | 292 | lcd.cls(); |
Faiyaz | 1:76c4a55fbac4 | 293 | } |
Faiyaz | 1:76c4a55fbac4 | 294 | |
Faiyaz | 1:76c4a55fbac4 | 295 | |
Faiyaz | 1:76c4a55fbac4 | 296 | /* |
malcolmlear | 0:4bc9e88c2cff | 297 | float temp = ReadTemp(); // get temperature |
malcolmlear | 0:4bc9e88c2cff | 298 | lcd.cls(); // clear lcd |
malcolmlear | 0:4bc9e88c2cff | 299 | lcd.printf("Temp = %f\n", temp); // print temperature |
malcolmlear | 0:4bc9e88c2cff | 300 | wait(1); // wait 1 second |
malcolmlear | 0:4bc9e88c2cff | 301 | lcd.cls(); // clear lcd |
Faiyaz | 1:76c4a55fbac4 | 302 | |
Faiyaz | 1:76c4a55fbac4 | 303 | |
malcolmlear | 0:4bc9e88c2cff | 304 | int swch = ReadSwitches(); // look at Switch states |
malcolmlear | 0:4bc9e88c2cff | 305 | lcd.printf("Switches = %d\n", swch); // print result |
Faiyaz | 1:76c4a55fbac4 | 306 | |
Faiyaz | 1:76c4a55fbac4 | 307 | |
malcolmlear | 0:4bc9e88c2cff | 308 | char Key = FindKeyChar(); // look for Key pressed |
malcolmlear | 0:4bc9e88c2cff | 309 | lcd.printf("Key = %c\n", Key); // print result |
malcolmlear | 0:4bc9e88c2cff | 310 | wait(1); // wait 1 second |
malcolmlear | 0:4bc9e88c2cff | 311 | lcd.cls(); // clear lcd |
Faiyaz | 1:76c4a55fbac4 | 312 | |
malcolmlear | 0:4bc9e88c2cff | 313 | int dist = ReadSonar(); // get distance |
malcolmlear | 0:4bc9e88c2cff | 314 | lcd.printf("Distance = %d\n", dist); // print result |
malcolmlear | 0:4bc9e88c2cff | 315 | lcd.printf("Servo = %f\n", spos); // print servo pos |
malcolmlear | 0:4bc9e88c2cff | 316 | wait(1); // wait 1 second |
Faiyaz | 1:76c4a55fbac4 | 317 | |
malcolmlear | 0:4bc9e88c2cff | 318 | ReadMotion(); // read new data in from the MPU-6050 |
malcolmlear | 0:4bc9e88c2cff | 319 | lcd.cls(); // clear lcd |
malcolmlear | 0:4bc9e88c2cff | 320 | lcd.locate(0,0); // print at start of first line |
malcolmlear | 0:4bc9e88c2cff | 321 | lcd.printf("x%.1f y%.1f z%.1f", Acceleration[0], Acceleration[1], Acceleration[2]); |
malcolmlear | 0:4bc9e88c2cff | 322 | lcd.locate(0,1); // print at start of second line |
malcolmlear | 0:4bc9e88c2cff | 323 | lcd.printf("x%.1f y%.1f z%.1f", GyroRate[0], GyroRate[1], GyroRate[2]); |
malcolmlear | 0:4bc9e88c2cff | 324 | wait(.4); // wait 0.4 second |
Faiyaz | 1:76c4a55fbac4 | 325 | |
malcolmlear | 0:4bc9e88c2cff | 326 | if (spos < 1) { // is servo at upper limit of 1 |
malcolmlear | 0:4bc9e88c2cff | 327 | spos += .1; // increment servo position |
malcolmlear | 0:4bc9e88c2cff | 328 | } // |
malcolmlear | 0:4bc9e88c2cff | 329 | else { // was at upper limit so |
malcolmlear | 0:4bc9e88c2cff | 330 | spos = -1; // reset servo position |
malcolmlear | 0:4bc9e88c2cff | 331 | } // |
malcolmlear | 0:4bc9e88c2cff | 332 | Servo1(spos); // update servo |
Faiyaz | 1:76c4a55fbac4 | 333 | */ |
malcolmlear | 0:4bc9e88c2cff | 334 | } |
malcolmlear | 0:4bc9e88c2cff | 335 | } |