University of Essex CE713
Dependencies: mbed HTTPServer EthernetNetIf TextLCD
Fork of HTTPServerHelloWorld by
main.cpp@6:7929de136a42, 2017-11-30 (annotated)
- Committer:
- malcolmlear
- Date:
- Thu Nov 30 15:30:41 2017 +0000
- Revision:
- 6:7929de136a42
- Parent:
- 5:8b8c719307bf
- Child:
- 7:7f4d940077bd
Change to sprintf formatting
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
malcolmlear | 4:7b810308854e | 1 | // Device Drivers for Labmbed Board |
malcolmlear | 4:7b810308854e | 2 | |
malcolmlear | 4:7b810308854e | 3 | #include "mbed.h" |
malcolmlear | 4:7b810308854e | 4 | #include "TextLCD.h" |
malcolmlear | 4:7b810308854e | 5 | #include "EthernetNetIf.h" |
malcolmlear | 4:7b810308854e | 6 | #include "HTTPServer.h" |
malcolmlear | 4:7b810308854e | 7 | |
malcolmlear | 4:7b810308854e | 8 | TextLCD lcd(p15, p16, p17, p18, p19, p20); // LCD: RS, E, D4-D7 |
malcolmlear | 4:7b810308854e | 9 | SPI spi(p5, p6, p7); // SPI: MOSI, MISO, SCLK (MISO not used with LCD) |
malcolmlear | 4:7b810308854e | 10 | DigitalOut lat(p8); // data latch for LED driver TLC59281 |
malcolmlear | 4:7b810308854e | 11 | DigitalOut Sel0(p26); // input select bits: |
malcolmlear | 4:7b810308854e | 12 | DigitalOut Sel1(p25); // " |
malcolmlear | 4:7b810308854e | 13 | DigitalOut Sel2(p24); // " |
malcolmlear | 4:7b810308854e | 14 | DigitalIn In0(p14); // input from switches, keypad etc |
malcolmlear | 4:7b810308854e | 15 | DigitalIn In1(p13); // " |
malcolmlear | 4:7b810308854e | 16 | DigitalIn In2(p12); // " |
malcolmlear | 4:7b810308854e | 17 | DigitalIn In3(p11); // " |
malcolmlear | 4:7b810308854e | 18 | I2C i2c(p9, p10); // I2C: SDA, SCL pins |
malcolmlear | 4:7b810308854e | 19 | PwmOut servo1(p21); // Servo 1 PWM pin |
malcolmlear | 4:7b810308854e | 20 | PwmOut servo2(p22); // Servo 2 PWM pin |
malcolmlear | 4:7b810308854e | 21 | DigitalOut Trigger(p28); // Ultrasonic rangefinder trigger pin |
malcolmlear | 4:7b810308854e | 22 | DigitalIn Echo(p27); // Ultrasonic rangefinder echo pin |
malcolmlear | 4:7b810308854e | 23 | Timer Sonar; // Ultrasonic timer |
malcolmlear | 4:7b810308854e | 24 | DigitalOut led1(LED1); // |
malcolmlear | 4:7b810308854e | 25 | Serial out(USBTX, USBRX); // |
malcolmlear | 4:7b810308854e | 26 | LocalFileSystem fs("webfs"); // |
malcolmlear | 4:7b810308854e | 27 | EthernetNetIf eth; // |
malcolmlear | 4:7b810308854e | 28 | HTTPServer svr; // |
malcolmlear | 4:7b810308854e | 29 | Ticker WebUpdate; // Interrupt timer to update web page |
malcolmlear | 4:7b810308854e | 30 | |
malcolmlear | 4:7b810308854e | 31 | // global variables |
malcolmlear | 4:7b810308854e | 32 | short LEDbits = 0; // global led status used for readback |
malcolmlear | 4:7b810308854e | 33 | const int TMP102Addr = 0x92; // TMP102 temperature I2C address |
malcolmlear | 4:7b810308854e | 34 | const int MPU6050Addr = 0xd0; // MPU-6050 accelerometer and Gyro I2C address |
malcolmlear | 4:7b810308854e | 35 | float Acceleration[3]; // MPU-6050 x,y,z acceleration values in 1G floating point |
malcolmlear | 4:7b810308854e | 36 | float GyroRate[3]; // MPU-6050 x,y,z gyrorates in degrees per second |
malcolmlear | 4:7b810308854e | 37 | float GyroOffset[3]; // MPU-6050 x,y,z gyrorates compensation |
malcolmlear | 4:7b810308854e | 38 | char AReg[] = { 0x3b, 0x3d, 0x3f }; // MPU-6050 I2C x,y,z accelerometer data registers |
malcolmlear | 4:7b810308854e | 39 | char GReg[] = { 0x43, 0x45, 0x47 }; // MPU-6050 I2C x,y,z gyro data registers |
malcolmlear | 4:7b810308854e | 40 | extern "C" void mbed_mac_address(char *mac); // |
malcolmlear | 4:7b810308854e | 41 | |
malcolmlear | 4:7b810308854e | 42 | void WebServerPoll () { |
malcolmlear | 4:7b810308854e | 43 | Net::poll(); |
malcolmlear | 4:7b810308854e | 44 | led1=!led1; //Show that we are alive |
malcolmlear | 4:7b810308854e | 45 | } |
malcolmlear | 4:7b810308854e | 46 | |
malcolmlear | 4:7b810308854e | 47 | int InitWebServer() { |
malcolmlear | 4:7b810308854e | 48 | EthernetErr ethErr = eth.setup(); |
malcolmlear | 5:8b8c719307bf | 49 | if(ethErr) { |
malcolmlear | 4:7b810308854e | 50 | return -1; |
malcolmlear | 4:7b810308854e | 51 | } |
malcolmlear | 4:7b810308854e | 52 | FILE *fp = fopen("/webfs/index.htm", "w"); // Open "out.txt" on the local file system for writing |
malcolmlear | 4:7b810308854e | 53 | fprintf(fp, "<html><head><title>Hello World online</title></head><body><h1>Hello World from Mbed NXP LPC1768!</h1></body></html>"); |
malcolmlear | 5:8b8c719307bf | 54 | fclose(fp); // Close file |
malcolmlear | 5:8b8c719307bf | 55 | FSHandler::mount("/webfs", "/files"); // Mount /webfs path on /files web path |
malcolmlear | 5:8b8c719307bf | 56 | FSHandler::mount("/webfs", "/"); // Mount /webfs path on web root path |
malcolmlear | 4:7b810308854e | 57 | svr.addHandler<SimpleHandler>("/hello"); |
malcolmlear | 4:7b810308854e | 58 | svr.addHandler<RPCHandler>("/rpc"); |
malcolmlear | 4:7b810308854e | 59 | svr.addHandler<FSHandler>("/files"); |
malcolmlear | 5:8b8c719307bf | 60 | svr.addHandler<FSHandler>("/"); // Default handler |
malcolmlear | 5:8b8c719307bf | 61 | svr.bind(80); // Example : http://155.245.21.144 |
malcolmlear | 5:8b8c719307bf | 62 | WebUpdate.attach(&WebServerPoll, 0.1); // update webserver every 100ms |
malcolmlear | 4:7b810308854e | 63 | return 0; // return without error |
malcolmlear | 4:7b810308854e | 64 | } |
malcolmlear | 4:7b810308854e | 65 | |
malcolmlear | 4:7b810308854e | 66 | void HTTPText(char t[100]) { |
malcolmlear | 4:7b810308854e | 67 | FILE *fp = fopen("/webfs/index.htm", "w"); // Open "out.txt" on the local file system for writing |
malcolmlear | 4:7b810308854e | 68 | fprintf(fp, t); |
malcolmlear | 4:7b810308854e | 69 | fclose(fp); |
malcolmlear | 4:7b810308854e | 70 | } |
malcolmlear | 4:7b810308854e | 71 | |
malcolmlear | 4:7b810308854e | 72 | void Servo1(float s) { // range +-1 |
malcolmlear | 4:7b810308854e | 73 | s=s+1; |
malcolmlear | 4:7b810308854e | 74 | if (s>=0 && s<=2) { |
malcolmlear | 4:7b810308854e | 75 | servo1.pulsewidth(s/2000+0.001); // pulse 1 - 2 ms |
malcolmlear | 4:7b810308854e | 76 | } |
malcolmlear | 4:7b810308854e | 77 | } |
malcolmlear | 4:7b810308854e | 78 | |
malcolmlear | 4:7b810308854e | 79 | void Servo2(float s) { // range +-1 |
malcolmlear | 4:7b810308854e | 80 | s=s+1; |
malcolmlear | 4:7b810308854e | 81 | if (s>=0 && s<=2) { |
malcolmlear | 4:7b810308854e | 82 | servo2.pulsewidth(s/2000+0.001); // pulse 1 - 2 ms |
malcolmlear | 4:7b810308854e | 83 | } |
malcolmlear | 4:7b810308854e | 84 | } |
malcolmlear | 4:7b810308854e | 85 | |
malcolmlear | 4:7b810308854e | 86 | void InitServos() { |
malcolmlear | 4:7b810308854e | 87 | servo1.period(0.02); |
malcolmlear | 4:7b810308854e | 88 | servo2.period(0.02); |
malcolmlear | 4:7b810308854e | 89 | Servo1(0); // initiate servo 1 to centre position |
malcolmlear | 4:7b810308854e | 90 | Servo2(0); // initiate servo 1 to centre position |
malcolmlear | 4:7b810308854e | 91 | } |
malcolmlear | 4:7b810308854e | 92 | |
malcolmlear | 4:7b810308854e | 93 | int ReadSonar() { |
malcolmlear | 4:7b810308854e | 94 | Trigger = 1; // set sonar trigger pulse high |
malcolmlear | 4:7b810308854e | 95 | Sonar.reset(); // reset sonar timer |
malcolmlear | 4:7b810308854e | 96 | wait_us(10.0); // 10 us pulse |
malcolmlear | 4:7b810308854e | 97 | Trigger = 0; // set sonar trigger pulse low |
malcolmlear | 4:7b810308854e | 98 | while (Echo == 0) {}; // wait for echo high (8 cycles have been transmitted) |
malcolmlear | 4:7b810308854e | 99 | Sonar.start(); // echo high so start timer |
malcolmlear | 4:7b810308854e | 100 | while (Echo == 1) {}; // wait for echo low |
malcolmlear | 4:7b810308854e | 101 | Sonar.stop(); // echo low so stop timer |
malcolmlear | 4:7b810308854e | 102 | return (Sonar.read_us()*10)/58; // read timer and scale to mm |
malcolmlear | 4:7b810308854e | 103 | } |
malcolmlear | 4:7b810308854e | 104 | |
malcolmlear | 4:7b810308854e | 105 | void InitLEDs() { |
malcolmlear | 4:7b810308854e | 106 | lat = 0; // latch must start low |
malcolmlear | 4:7b810308854e | 107 | spi.format(16,0); // SPI 16 bit data, low state, high going clock |
malcolmlear | 4:7b810308854e | 108 | spi.frequency(1000000); // 1MHz clock rate |
malcolmlear | 4:7b810308854e | 109 | } |
malcolmlear | 4:7b810308854e | 110 | |
malcolmlear | 4:7b810308854e | 111 | void SetLEDs(short ledall) { |
malcolmlear | 4:7b810308854e | 112 | LEDbits = ledall; // update global led status |
malcolmlear | 4:7b810308854e | 113 | spi.write((LEDbits & 0x03ff) | ((LEDbits & 0xa800) >> 1) | ((LEDbits & 0x5400) << 1)); |
malcolmlear | 4:7b810308854e | 114 | lat = 1; // latch pulse start |
malcolmlear | 4:7b810308854e | 115 | lat = 0; // latch pulse end |
malcolmlear | 4:7b810308854e | 116 | } |
malcolmlear | 4:7b810308854e | 117 | |
malcolmlear | 4:7b810308854e | 118 | void SetLED(short LEDNo, short LEDState) { |
malcolmlear | 4:7b810308854e | 119 | LEDNo = ((LEDNo - 1) & 0x0007) + 1; // limit led number |
malcolmlear | 4:7b810308854e | 120 | LEDState = LEDState & 0x0003; // limit led state |
malcolmlear | 4:7b810308854e | 121 | LEDNo = (8 - LEDNo) * 2; // offset of led state in 'LEDbits' |
malcolmlear | 4:7b810308854e | 122 | LEDState = LEDState << LEDNo; |
malcolmlear | 4:7b810308854e | 123 | short statemask = ((0x0003 << LEDNo) ^ 0xffff); // mask used to clear led state |
malcolmlear | 4:7b810308854e | 124 | LEDbits = ((LEDbits & statemask) | LEDState); // clear and set led state |
malcolmlear | 4:7b810308854e | 125 | SetLEDs(LEDbits); |
malcolmlear | 4:7b810308854e | 126 | } |
malcolmlear | 4:7b810308854e | 127 | |
malcolmlear | 4:7b810308854e | 128 | short ReadLED(short LEDNo) { |
malcolmlear | 4:7b810308854e | 129 | LEDNo = ((LEDNo - 1) & 0x0007) + 1; // limit led number |
malcolmlear | 4:7b810308854e | 130 | LEDNo = (8 - LEDNo) * 2; // offset of led state in 'LEDbits' |
malcolmlear | 4:7b810308854e | 131 | short LEDState = (LEDbits >> LEDNo) & 0x0003; // shift selected led state into ls 2 bits |
malcolmlear | 4:7b810308854e | 132 | return LEDState; // return led state |
malcolmlear | 4:7b810308854e | 133 | } |
malcolmlear | 4:7b810308854e | 134 | |
malcolmlear | 4:7b810308854e | 135 | short ReadLEDs() { |
malcolmlear | 4:7b810308854e | 136 | return LEDbits; // return led status |
malcolmlear | 4:7b810308854e | 137 | } |
malcolmlear | 4:7b810308854e | 138 | |
malcolmlear | 4:7b810308854e | 139 | void SelInput(short Input) { |
malcolmlear | 4:7b810308854e | 140 | Sel0 = Input & 0x0001; // set sel[0:2] pins |
malcolmlear | 4:7b810308854e | 141 | Sel1 = (Input >> 1) & 0x0001; // |
malcolmlear | 4:7b810308854e | 142 | Sel2 = (Input >> 2) & 0x0001; // |
malcolmlear | 4:7b810308854e | 143 | } |
malcolmlear | 4:7b810308854e | 144 | |
malcolmlear | 4:7b810308854e | 145 | short ReadSwitches() { |
malcolmlear | 4:7b810308854e | 146 | SelInput(5); // select least significant 4 switches in[3:0] |
malcolmlear | 4:7b810308854e | 147 | short Switches = In0 + (In1 << 1) + (In2 << 2) + (In3 << 3); |
malcolmlear | 4:7b810308854e | 148 | SelInput(4); // select most significant 4 switches in[3:0] |
malcolmlear | 4:7b810308854e | 149 | return (Switches + (In0 << 4) + (In1 << 5) + (In2 << 6) + (In3 << 7)); |
malcolmlear | 4:7b810308854e | 150 | } |
malcolmlear | 4:7b810308854e | 151 | |
malcolmlear | 4:7b810308854e | 152 | short ReadSwitch(short SwitchNo) { |
malcolmlear | 4:7b810308854e | 153 | SwitchNo = ((SwitchNo - 1) & 0x0007) + 1; // limit switch number |
malcolmlear | 4:7b810308854e | 154 | SwitchNo = 8 - SwitchNo; // offset of switch state in ReadSwitches() |
malcolmlear | 4:7b810308854e | 155 | short SwitchState = ReadSwitches(); // read switch states |
malcolmlear | 4:7b810308854e | 156 | SwitchState = SwitchState >> SwitchNo; // shift selected switch state into ls bit |
malcolmlear | 4:7b810308854e | 157 | return (SwitchState & 0x0001); // mask out and return switch state |
malcolmlear | 4:7b810308854e | 158 | } |
malcolmlear | 4:7b810308854e | 159 | |
malcolmlear | 4:7b810308854e | 160 | short ReadKeys() { |
malcolmlear | 4:7b810308854e | 161 | SelInput(0); // select Keypad top row |
malcolmlear | 4:7b810308854e | 162 | short Keys = (In0 << 15) + (In1 << 14) + (In2 << 13) + (In3 << 12); |
malcolmlear | 4:7b810308854e | 163 | SelInput(1); // select Keypad second row |
malcolmlear | 4:7b810308854e | 164 | Keys += (In0 << 3) + (In1 << 6) + (In2 << 9) + (In3 << 11); |
malcolmlear | 4:7b810308854e | 165 | SelInput(2); // select Keypad third row |
malcolmlear | 4:7b810308854e | 166 | Keys += (In0 << 2) + (In1 << 5) + (In2 << 8) + In3; |
malcolmlear | 4:7b810308854e | 167 | SelInput(3); // select Keypad forth row |
malcolmlear | 4:7b810308854e | 168 | Keys += (In0 << 1) + (In1 << 4) + (In2 << 7) + (In3 << 10); |
malcolmlear | 4:7b810308854e | 169 | return (Keys ^ 0xffff); // return inverted (Key press active high) |
malcolmlear | 4:7b810308854e | 170 | } |
malcolmlear | 4:7b810308854e | 171 | |
malcolmlear | 4:7b810308854e | 172 | short ReadKey(short KeyNo) { |
malcolmlear | 4:7b810308854e | 173 | KeyNo = KeyNo & 0x000f; // limit key number 0 to 15 (0 to F) |
malcolmlear | 4:7b810308854e | 174 | short KeyState = ReadKeys(); // read key states |
malcolmlear | 4:7b810308854e | 175 | KeyState = KeyState >> KeyNo; // shift selected key state into ls bit |
malcolmlear | 4:7b810308854e | 176 | return (KeyState & 0x0001); // mask out and return key state |
malcolmlear | 4:7b810308854e | 177 | } |
malcolmlear | 4:7b810308854e | 178 | |
malcolmlear | 4:7b810308854e | 179 | int FindKeyNo() { |
malcolmlear | 4:7b810308854e | 180 | short KeyNo; |
malcolmlear | 4:7b810308854e | 181 | short KeyPressed = -1; // set KeyPressed to -1 (no key pressed) |
malcolmlear | 4:7b810308854e | 182 | short KeyState = ReadKeys(); // read key states |
malcolmlear | 4:7b810308854e | 183 | for (KeyNo= 0; KeyNo < 16; KeyNo++ ) { // check all 16 Keys |
malcolmlear | 4:7b810308854e | 184 | if (KeyState & 0x0001) { // check key state |
malcolmlear | 4:7b810308854e | 185 | if (KeyPressed == -1) { // check if key already found |
malcolmlear | 4:7b810308854e | 186 | KeyPressed = KeyNo; // update KeyPressed |
malcolmlear | 4:7b810308854e | 187 | } |
malcolmlear | 4:7b810308854e | 188 | else { |
malcolmlear | 4:7b810308854e | 189 | return -1; // 2 or more keys pressed |
malcolmlear | 4:7b810308854e | 190 | } |
malcolmlear | 4:7b810308854e | 191 | } |
malcolmlear | 4:7b810308854e | 192 | KeyState = KeyState >> 1; // shift to check next key |
malcolmlear | 4:7b810308854e | 193 | } |
malcolmlear | 4:7b810308854e | 194 | return KeyPressed; // return KeyPressed |
malcolmlear | 4:7b810308854e | 195 | } |
malcolmlear | 4:7b810308854e | 196 | |
malcolmlear | 4:7b810308854e | 197 | char FindKeyChar() { |
malcolmlear | 4:7b810308854e | 198 | short KeyNo; |
malcolmlear | 4:7b810308854e | 199 | char KeyChar = ' '; // set KeyChar to ' ' (no key pressed) |
malcolmlear | 4:7b810308854e | 200 | KeyNo = FindKeyNo(); // find key pressed |
malcolmlear | 4:7b810308854e | 201 | if (KeyNo < 10 && KeyNo >= 0) { |
malcolmlear | 4:7b810308854e | 202 | KeyChar = (char) KeyNo + 0x30; // convert char 0-9 to ascii string '0'-'9' |
malcolmlear | 4:7b810308854e | 203 | } |
malcolmlear | 4:7b810308854e | 204 | if (KeyNo > 9 && KeyNo < 16) { |
malcolmlear | 4:7b810308854e | 205 | KeyChar = (char) KeyNo + 0x37; // convert char 10-15 to ascii string 'A'-'F' |
malcolmlear | 4:7b810308854e | 206 | } |
malcolmlear | 4:7b810308854e | 207 | return KeyChar; // return key pressed |
malcolmlear | 4:7b810308854e | 208 | } |
malcolmlear | 4:7b810308854e | 209 | |
malcolmlear | 4:7b810308854e | 210 | float ReadTemp() { |
malcolmlear | 4:7b810308854e | 211 | char Cmd[3]; |
malcolmlear | 4:7b810308854e | 212 | Cmd[0] = 0x01; // pointer register value |
malcolmlear | 4:7b810308854e | 213 | Cmd[1] = 0x60; // byte 1 of the configuration register |
malcolmlear | 4:7b810308854e | 214 | Cmd[2] = 0xa0; // byte 2 of the configuration register |
malcolmlear | 4:7b810308854e | 215 | i2c.write(TMP102Addr, Cmd, 3); // select configuration register and write 0x60a0 to it |
malcolmlear | 4:7b810308854e | 216 | wait(0.5); // ensure conversion time |
malcolmlear | 4:7b810308854e | 217 | Cmd[0] = 0x00; // pointer register value |
malcolmlear | 4:7b810308854e | 218 | i2c.write(TMP102Addr, Cmd, 1); // select temperature register |
malcolmlear | 4:7b810308854e | 219 | i2c.read(TMP102Addr, Cmd, 2); // read 16-bit temperature register |
malcolmlear | 4:7b810308854e | 220 | return (float((Cmd[0] << 8) | Cmd[1]) / 256); // divide by 256 and return temperature |
malcolmlear | 4:7b810308854e | 221 | } |
malcolmlear | 4:7b810308854e | 222 | |
malcolmlear | 4:7b810308854e | 223 | signed short ReadMPU6050(int RegAddr) { |
malcolmlear | 4:7b810308854e | 224 | char Cmd[3]; |
malcolmlear | 4:7b810308854e | 225 | Cmd[0] = RegAddr; // register address |
malcolmlear | 4:7b810308854e | 226 | i2c.write(MPU6050Addr, Cmd, 1); // select register to read |
malcolmlear | 4:7b810308854e | 227 | i2c.read(MPU6050Addr, Cmd, 2); // read 2 bytes from register |
malcolmlear | 4:7b810308854e | 228 | return ((Cmd[0] << 8) | Cmd[1]); // return signed 16 bit value |
malcolmlear | 4:7b810308854e | 229 | } |
malcolmlear | 4:7b810308854e | 230 | |
malcolmlear | 4:7b810308854e | 231 | void CalibrateGyros() { |
malcolmlear | 4:7b810308854e | 232 | short a,b; |
malcolmlear | 4:7b810308854e | 233 | for(a=0; a<3; a++) { |
malcolmlear | 4:7b810308854e | 234 | GyroOffset[a] = 0; // clear gyro calibration offsets |
malcolmlear | 4:7b810308854e | 235 | for(b=0; b<1000; b++) { |
malcolmlear | 4:7b810308854e | 236 | GyroOffset[a] = GyroOffset[a] + (float)ReadMPU6050(GReg[a]); |
malcolmlear | 4:7b810308854e | 237 | wait_ms(1); // wait for next sample |
malcolmlear | 4:7b810308854e | 238 | } |
malcolmlear | 4:7b810308854e | 239 | GyroOffset[a] = GyroOffset[a]/1000; // find average over 1000 samples |
malcolmlear | 4:7b810308854e | 240 | } |
malcolmlear | 4:7b810308854e | 241 | } |
malcolmlear | 4:7b810308854e | 242 | |
malcolmlear | 4:7b810308854e | 243 | void InitMotion() { |
malcolmlear | 4:7b810308854e | 244 | char Cmd[3]; |
malcolmlear | 4:7b810308854e | 245 | Cmd[0] = 0xa1; // config register address |
malcolmlear | 4:7b810308854e | 246 | Cmd[1] = 0x06; // accelerometer and gyro bandwidth = 5Hz |
malcolmlear | 4:7b810308854e | 247 | i2c.write(MPU6050Addr, Cmd, 2); // write data to config register |
malcolmlear | 4:7b810308854e | 248 | Cmd[0] = 0x6b; // power management register address |
malcolmlear | 4:7b810308854e | 249 | Cmd[1] = 0x00; // data |
malcolmlear | 4:7b810308854e | 250 | i2c.write(MPU6050Addr, Cmd, 2); // write data to power management register |
malcolmlear | 4:7b810308854e | 251 | Cmd[0] = 0x1b; // gyro configuration register address |
malcolmlear | 4:7b810308854e | 252 | Cmd[1] = 0x08; // no gyro self test, +-500 full scale |
malcolmlear | 4:7b810308854e | 253 | i2c.write(MPU6050Addr, Cmd, 2); // write data to gyro configuration register |
malcolmlear | 4:7b810308854e | 254 | Cmd[0] = 0x19; // sample rate register address |
malcolmlear | 4:7b810308854e | 255 | Cmd[1] = 0x07; // sample rate = gyro output rate / 8 |
malcolmlear | 4:7b810308854e | 256 | i2c.write(MPU6050Addr, Cmd, 2); // write data to sample rate register |
malcolmlear | 4:7b810308854e | 257 | CalibrateGyros(); |
malcolmlear | 4:7b810308854e | 258 | } |
malcolmlear | 4:7b810308854e | 259 | |
malcolmlear | 4:7b810308854e | 260 | void ReadMotion() { |
malcolmlear | 4:7b810308854e | 261 | short a; // Acceleration is in G where 1G = 9.81 ms/s |
malcolmlear | 4:7b810308854e | 262 | for(a=0; a<3; a++) { // GyroRate is in degrees per second |
malcolmlear | 4:7b810308854e | 263 | Acceleration[a] = (float)ReadMPU6050(AReg[a]) / 16384; |
malcolmlear | 4:7b810308854e | 264 | GyroRate[a] = ((float)ReadMPU6050(GReg[a]) - GyroOffset[a]) / 66.5; |
malcolmlear | 4:7b810308854e | 265 | } |
malcolmlear | 4:7b810308854e | 266 | } |
malcolmlear | 4:7b810308854e | 267 | |
malcolmlear | 4:7b810308854e | 268 | int main() { |
malcolmlear | 4:7b810308854e | 269 | |
malcolmlear | 4:7b810308854e | 270 | float spos = 0; // Test servo position |
malcolmlear | 4:7b810308854e | 271 | char mac[6]; // MAC address |
malcolmlear | 4:7b810308854e | 272 | InitLEDs(); // |
malcolmlear | 4:7b810308854e | 273 | InitMotion(); // |
malcolmlear | 4:7b810308854e | 274 | InitServos(); // |
malcolmlear | 4:7b810308854e | 275 | InitWebServer(); // |
malcolmlear | 4:7b810308854e | 276 | |
malcolmlear | 4:7b810308854e | 277 | while(1) { |
malcolmlear | 4:7b810308854e | 278 | int a,b; |
malcolmlear | 4:7b810308854e | 279 | for (b = 0; b < 4; b++ ) { // select all 4 led states |
malcolmlear | 4:7b810308854e | 280 | for (a = 1; a < 9; a++ ) { // set all 8 leds to selected state |
malcolmlear | 4:7b810308854e | 281 | SetLED (a,b); // set led 'a' to state 'b' |
malcolmlear | 4:7b810308854e | 282 | wait(.05); // wait 0.05 second |
malcolmlear | 4:7b810308854e | 283 | } |
malcolmlear | 4:7b810308854e | 284 | } |
malcolmlear | 4:7b810308854e | 285 | for (a= 1; a < 9; a++ ) { // map Switch states to led's |
malcolmlear | 4:7b810308854e | 286 | SetLED (a,(ReadSwitch(a) + 1)); // |
malcolmlear | 4:7b810308854e | 287 | wait(.05); // wait 0.05 second |
malcolmlear | 4:7b810308854e | 288 | } |
malcolmlear | 4:7b810308854e | 289 | float temp = ReadTemp(); // get temperature |
malcolmlear | 4:7b810308854e | 290 | lcd.cls(); // clear lcd |
malcolmlear | 4:7b810308854e | 291 | lcd.printf("Temp = %f\n", temp); // print temperature |
malcolmlear | 4:7b810308854e | 292 | wait(1); // wait 1 second |
malcolmlear | 4:7b810308854e | 293 | lcd.cls(); // clear lcd |
malcolmlear | 4:7b810308854e | 294 | int swch = ReadSwitches(); // look at Switch states |
malcolmlear | 4:7b810308854e | 295 | lcd.printf("Switches = %d\n", swch); // print result |
malcolmlear | 4:7b810308854e | 296 | char Key = FindKeyChar(); // look for Key pressed |
malcolmlear | 4:7b810308854e | 297 | lcd.printf("Key = %c\n", Key); // print result |
malcolmlear | 4:7b810308854e | 298 | wait(1); // wait 1 second |
malcolmlear | 4:7b810308854e | 299 | lcd.cls(); // clear lcd |
malcolmlear | 4:7b810308854e | 300 | int dist = ReadSonar(); // get distance |
malcolmlear | 4:7b810308854e | 301 | lcd.printf("Distance = %d\n", dist); // print result |
malcolmlear | 4:7b810308854e | 302 | lcd.printf("Servo = %f\n", spos); // print servo pos |
malcolmlear | 4:7b810308854e | 303 | wait(1); // wait 1 second |
malcolmlear | 4:7b810308854e | 304 | ReadMotion(); // read new data in from the MPU-6050 |
malcolmlear | 4:7b810308854e | 305 | lcd.cls(); // clear lcd |
malcolmlear | 4:7b810308854e | 306 | lcd.locate(0,0); // print at start of first line |
malcolmlear | 4:7b810308854e | 307 | lcd.printf("x%.1f y%.1f z%.1f", Acceleration[0], Acceleration[1], Acceleration[2]); |
malcolmlear | 4:7b810308854e | 308 | lcd.locate(0,1); // print at start of second line |
malcolmlear | 4:7b810308854e | 309 | lcd.printf("x%.1f y%.1f z%.1f", GyroRate[0], GyroRate[1], GyroRate[2]); |
malcolmlear | 4:7b810308854e | 310 | wait(1); // wait 1 second |
malcolmlear | 4:7b810308854e | 311 | lcd.cls(); // clear lcd |
malcolmlear | 6:7929de136a42 | 312 | lcd.printf("MAC Address = "); // print to LCD |
malcolmlear | 6:7929de136a42 | 313 | mbed_mac_address(mac); // get MAC address |
malcolmlear | 6:7929de136a42 | 314 | for(int i=0; i<6;i++) { // step through MAC address characters |
malcolmlear | 6:7929de136a42 | 315 | lcd.printf("%02X ", mac[i]); // print MAC address |
malcolmlear | 4:7b810308854e | 316 | } |
malcolmlear | 4:7b810308854e | 317 | printf("\n"); |
malcolmlear | 4:7b810308854e | 318 | wait(.4); // wait 0.4 second |
malcolmlear | 4:7b810308854e | 319 | if (spos < 1) { // is servo at upper limit of 1 |
malcolmlear | 4:7b810308854e | 320 | spos += .1; // increment servo position |
malcolmlear | 4:7b810308854e | 321 | } // |
malcolmlear | 4:7b810308854e | 322 | else { // was at upper limit so |
malcolmlear | 4:7b810308854e | 323 | spos = -1; // reset servo position |
malcolmlear | 4:7b810308854e | 324 | } // |
malcolmlear | 4:7b810308854e | 325 | Servo1(spos); // update servo |
malcolmlear | 5:8b8c719307bf | 326 | // HTTPText("<html><head><title>Hello World online</title></head><body><h1>New Message !!!!</h1></body></html>"); |
malcolmlear | 5:8b8c719307bf | 327 | char Buffer[100]; |
malcolmlear | 6:7929de136a42 | 328 | sprintf(Buffer, "<html><head><title>Hello World online</title></head><body><h1> Temperature = %.3f %s", temp, "</h1></body></html>"); |
malcolmlear | 5:8b8c719307bf | 329 | HTTPText(Buffer); // ouput temperature on web page |
malcolmlear | 4:7b810308854e | 330 | } |
malcolmlear | 4:7b810308854e | 331 | } |