Chandan Siyag / Mbed 2 deprecated AlooMBED

3rd year group project. Electronic and Electrical Engineering. Heriot-Watt University. This is the code for the mbed for the Automatic Little Object Organiser (ALOO).

Dependencies:   MCP23017 TCS3472_I2C WattBob_TextLCD mbed

main.cpp@30:c0bc92d009fe, 2015-12-01 (annotated)

Committer:
dreamselec
Date:
Tue Dec 01 23:50:29 2015 +0000
Revision:
30:c0bc92d009fe
Parent:
29:9c0339e3c593
Child:
31:16e056b9f9e0
All functions appear to be ... functioning. Need to test set new haz block mode with hardware.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
dreamselec 0:fe5cb0a8fc5a 1 #include "mbed.h"
dreamselec 0:fe5cb0a8fc5a 2 #include "WattBob_TextLCD.h"
dreamselec 0:fe5cb0a8fc5a 3 #include "TCS3472_I2C.h"
dreamselec 0:fe5cb0a8fc5a 4 #include "MCP23017.h"
dreamselec 0:fe5cb0a8fc5a 5 #include <string>
dreamselec 1:92a2a5ef65a8 6 #include <time.h>
dreamselec 2:7a55cb10259f 7 //#include <future>
dreamselec 2:7a55cb10259f 8 #include "globals.h"
dreamselec 2:7a55cb10259f 9 #include "commander.h"
dreamselec 3:843b830ee8bd 10 #include "fpga.h"
dreamselec 0:fe5cb0a8fc5a 11
dreamselec 0:fe5cb0a8fc5a 12 #define BACKLIGHT_ON(INTERFACE) INTERFACE->write_bit(1, 4);
dreamselec 0:fe5cb0a8fc5a 13 #define BACKLIGHT_OFF(INTERFACE) INTERFACE->write_bit(0, 4);
dreamselec 0:fe5cb0a8fc5a 14
dreamselec 0:fe5cb0a8fc5a 15 #define LCDFL() lcd->locate(0,0);
dreamselec 0:fe5cb0a8fc5a 16 #define LCDSL() lcd->locate(1,0);
dreamselec 2:7a55cb10259f 17 #define D_LEDS_OFF() i2cport->write_bit(0, 12); i2cport->write_bit(0, 13); i2cport->write_bit(0, 14); i2cport->write_bit(0, 15);
dreamselec 2:7a55cb10259f 18 #define U_LEDS_OFF() myLED1 = 0; myLED2 = 0; myLED3 = 0; myLED4 = 0;
dreamselec 0:fe5cb0a8fc5a 19
dreamselec 2:7a55cb10259f 20 DigitalOut myLED1(LED1);
dreamselec 2:7a55cb10259f 21 DigitalOut myLED2(LED2);
dreamselec 2:7a55cb10259f 22 DigitalOut myLED3(LED3);
dreamselec 2:7a55cb10259f 23 DigitalOut myLED4(LED4);
dreamselec 0:fe5cb0a8fc5a 24
dreamselec 0:fe5cb0a8fc5a 25 MCP23017 *i2cport;
dreamselec 0:fe5cb0a8fc5a 26 WattBob_TextLCD *lcd;
dreamselec 10:16ba52f8e025 27 TCS3472_I2C rgbSensor(p28, p27);
dreamselec 2:7a55cb10259f 28 Serial pc(USBTX, USBRX);
dreamselec 8:e1da2ae62885 29 uint8_t rxBuffer[kSmallBufferSize + 1];
dreamselec 8:e1da2ae62885 30 int rxIndex = 0;
dreamselec 21:60c4fe94d79c 31 float percentageError[3];
dreamselec 21:60c4fe94d79c 32 float adjustedValues[3];
dreamselec 27:2cb1bdb7ae3d 33 int blockCount = 0;
dreamselec 27:2cb1bdb7ae3d 34 bool lastBlockHaz = false;
dreamselec 28:7e4d29977d72 35 bool recievingResponse = false;
dreamselec 30:c0bc92d009fe 36 bool isBetweenHazValues[3] = { false, false, false };
dreamselec 2:7a55cb10259f 37
dreamselec 6:98fe30430194 38 Commander _commander = Commander();
dreamselec 6:98fe30430194 39 Commander *commander = &_commander;
dreamselec 6:98fe30430194 40 FPGA _fpga = FPGA();
dreamselec 9:dc8f155b71c8 41 extern FPGA *fpga = &_fpga;
dreamselec 6:98fe30430194 42
dreamselec 20:4e0f0944f28f 43 Block defualtHazBlock = Block();
dreamselec 6:98fe30430194 44 extern Block _HazBlock;
dreamselec 6:98fe30430194 45 Block *HazBlock = &_HazBlock;
dreamselec 6:98fe30430194 46
dreamselec 18:44a1c1a30166 47 extern PCModes currentMode;
dreamselec 18:44a1c1a30166 48
dreamselec 6:98fe30430194 49 void initInternal();
dreamselec 7:b6e31bfdb2af 50 void initPort(int baudRate=kDefaultBaudRate);
dreamselec 6:98fe30430194 51 void printPCDetectedText();
dreamselec 6:98fe30430194 52 void Rx_interrupt();
dreamselec 18:44a1c1a30166 53 bool checkColour(int colourValues[]);
dreamselec 10:16ba52f8e025 54 void runInServoTestMode();
dreamselec 15:777390eb5afd 55 void displayWaitingLine();
dreamselec 15:777390eb5afd 56 void displayPCStatus();
dreamselec 18:44a1c1a30166 57 bool waitForBlock();
dreamselec 18:44a1c1a30166 58 void sortBlock();
dreamselec 23:db91aaa43a9e 59 void runInBreakBeamTestMode();
dreamselec 23:db91aaa43a9e 60 void turnOffTopLEDs();
dreamselec 23:db91aaa43a9e 61 void turnOffBottomLEDs();
dreamselec 25:792540d69c49 62 void runInColourSensorTestMode();
dreamselec 30:c0bc92d009fe 63 void newHazBlockMode();
dreamselec 6:98fe30430194 64
dreamselec 6:98fe30430194 65 int main()
dreamselec 6:98fe30430194 66 {
dreamselec 27:2cb1bdb7ae3d 67 initInternal();
dreamselec 27:2cb1bdb7ae3d 68 initPort();
dreamselec 27:2cb1bdb7ae3d 69
dreamselec 27:2cb1bdb7ae3d 70 for (;;) {
dreamselec 27:2cb1bdb7ae3d 71 if (connectedToPC == false) {
dreamselec 28:7e4d29977d72 72 pc.printf("DEBUG: PC did not responded.\n");
dreamselec 27:2cb1bdb7ae3d 73 lcd->cls();
dreamselec 27:2cb1bdb7ae3d 74 i2cport->write_bit(1, 12);
dreamselec 27:2cb1bdb7ae3d 75 lcd->locate(0,0);
dreamselec 27:2cb1bdb7ae3d 76 lcd->printf("1: Start sorting.");
dreamselec 27:2cb1bdb7ae3d 77 lcd->locate(1,0);
dreamselec 27:2cb1bdb7ae3d 78 i2cport->write_bit(1,13);
dreamselec 27:2cb1bdb7ae3d 79 lcd->printf("2: Connect to PC");
dreamselec 27:2cb1bdb7ae3d 80 }
dreamselec 27:2cb1bdb7ae3d 81
dreamselec 24:02c61793f90b 82 // wait(0.03);
dreamselec 10:16ba52f8e025 83
dreamselec 27:2cb1bdb7ae3d 84 int selection = 0;
dreamselec 27:2cb1bdb7ae3d 85 do {
dreamselec 27:2cb1bdb7ae3d 86 myLED4 = selection;
dreamselec 27:2cb1bdb7ae3d 87 selection = readSwitches();
dreamselec 27:2cb1bdb7ae3d 88 } while (selection != 1 && selection != 2 && connectedToPC == false);
dreamselec 27:2cb1bdb7ae3d 89 // lcd->cls();
dreamselec 27:2cb1bdb7ae3d 90 D_LEDS_OFF();
dreamselec 27:2cb1bdb7ae3d 91 if (selection == 1) {
dreamselec 27:2cb1bdb7ae3d 92 // User selected op 1: Start sorting autonomously.
dreamselec 27:2cb1bdb7ae3d 93 i2cport->write_bit(1, 12);
dreamselec 27:2cb1bdb7ae3d 94 lcd->cls();
dreamselec 27:2cb1bdb7ae3d 95 LCDFL();
dreamselec 27:2cb1bdb7ae3d 96 lcd->printf("Starting sorting");
dreamselec 27:2cb1bdb7ae3d 97 wait(0.5);
dreamselec 27:2cb1bdb7ae3d 98 D_LEDS_OFF();
dreamselec 19:61b21ac4896e 99
dreamselec 27:2cb1bdb7ae3d 100 for(;;) {
dreamselec 27:2cb1bdb7ae3d 101 if (blockCount > 0) {
dreamselec 27:2cb1bdb7ae3d 102 lcd->cls();
dreamselec 27:2cb1bdb7ae3d 103 displayWaitingLine();
dreamselec 27:2cb1bdb7ae3d 104 lcd->locate(1,0);
dreamselec 27:2cb1bdb7ae3d 105 if (lastBlockHaz == true)
dreamselec 27:2cb1bdb7ae3d 106 lcd->printf("Hazardous");
dreamselec 27:2cb1bdb7ae3d 107 else if (lastBlockHaz == false)
dreamselec 27:2cb1bdb7ae3d 108 lcd->printf("Non-Hazardous");
dreamselec 27:2cb1bdb7ae3d 109 } else {
dreamselec 27:2cb1bdb7ae3d 110 displayWaitingLine();
dreamselec 27:2cb1bdb7ae3d 111 }
dreamselec 10:16ba52f8e025 112
dreamselec 27:2cb1bdb7ae3d 113 i2cport->write_bit(1, 15);
dreamselec 23:db91aaa43a9e 114
dreamselec 27:2cb1bdb7ae3d 115 // Break and return to main menu i.e. Start Op, or Connect to PC.
dreamselec 27:2cb1bdb7ae3d 116 if (waitForBlock() == false) {
dreamselec 27:2cb1bdb7ae3d 117 D_LEDS_OFF();
dreamselec 27:2cb1bdb7ae3d 118 break;
dreamselec 27:2cb1bdb7ae3d 119 } else {
dreamselec 27:2cb1bdb7ae3d 120 sortBlock();
dreamselec 27:2cb1bdb7ae3d 121 }
dreamselec 27:2cb1bdb7ae3d 122 }
dreamselec 27:2cb1bdb7ae3d 123 }
dreamselec 10:16ba52f8e025 124
dreamselec 27:2cb1bdb7ae3d 125 if (selection == 2 || connectedToPC == true) {
dreamselec 27:2cb1bdb7ae3d 126 wait(0.1);
dreamselec 27:2cb1bdb7ae3d 127 for (;;) {
dreamselec 27:2cb1bdb7ae3d 128 displayPCStatus();
dreamselec 19:61b21ac4896e 129
dreamselec 27:2cb1bdb7ae3d 130 i2cport->write_bit(1, 15);
dreamselec 27:2cb1bdb7ae3d 131 int abortOperation = false;
dreamselec 27:2cb1bdb7ae3d 132 if (connectedToPC == false)
dreamselec 27:2cb1bdb7ae3d 133 pc.printf(":<pc>connect;");
dreamselec 27:2cb1bdb7ae3d 134 while (connectedToPC == false && abortOperation == false) {
dreamselec 27:2cb1bdb7ae3d 135 abortOperation = readSwitches() == 4;
dreamselec 27:2cb1bdb7ae3d 136 }
dreamselec 18:44a1c1a30166 137
dreamselec 27:2cb1bdb7ae3d 138 if (abortOperation == true) {
dreamselec 27:2cb1bdb7ae3d 139 D_LEDS_OFF();
dreamselec 27:2cb1bdb7ae3d 140 break;
dreamselec 27:2cb1bdb7ae3d 141 }
dreamselec 27:2cb1bdb7ae3d 142
dreamselec 27:2cb1bdb7ae3d 143 displayPCStatus();
dreamselec 15:777390eb5afd 144
dreamselec 27:2cb1bdb7ae3d 145 while (abortOperation == false && connectedToPC == true) {
dreamselec 27:2cb1bdb7ae3d 146 if (currentMode == Maintanence) {
dreamselec 27:2cb1bdb7ae3d 147 displayPCStatus();
dreamselec 27:2cb1bdb7ae3d 148 while (currentMode == Maintanence) {
dreamselec 27:2cb1bdb7ae3d 149 if (runServoTest == true)
dreamselec 27:2cb1bdb7ae3d 150 runInServoTestMode();
dreamselec 27:2cb1bdb7ae3d 151 else if (runBreakBeamTest == true)
dreamselec 27:2cb1bdb7ae3d 152 runInBreakBeamTestMode();
dreamselec 27:2cb1bdb7ae3d 153 else if (runColourSensorTest == true)
dreamselec 27:2cb1bdb7ae3d 154 runInColourSensorTestMode();
dreamselec 30:c0bc92d009fe 155 else if (setNewHazBlock == true)
dreamselec 30:c0bc92d009fe 156 newHazBlockMode();
dreamselec 27:2cb1bdb7ae3d 157 if (i2cport->read_bit(11) == 1) {
dreamselec 27:2cb1bdb7ae3d 158 if (displayAbortDialog() == true) {
dreamselec 27:2cb1bdb7ae3d 159 currentMode = None;
dreamselec 27:2cb1bdb7ae3d 160 pc.printf(":<mbed>mode=none;");
dreamselec 27:2cb1bdb7ae3d 161 } else {
dreamselec 27:2cb1bdb7ae3d 162 displayPCStatus();
dreamselec 27:2cb1bdb7ae3d 163 }
dreamselec 27:2cb1bdb7ae3d 164 }
dreamselec 27:2cb1bdb7ae3d 165 }
dreamselec 27:2cb1bdb7ae3d 166 } else if (currentMode == Normal) {
dreamselec 27:2cb1bdb7ae3d 167 displayPCStatus();
dreamselec 27:2cb1bdb7ae3d 168 while (currentMode == Normal) {
dreamselec 27:2cb1bdb7ae3d 169 if (currentState == Pause) {
dreamselec 27:2cb1bdb7ae3d 170 lcd->cls();
dreamselec 27:2cb1bdb7ae3d 171 lcd->locate(0,0);
dreamselec 27:2cb1bdb7ae3d 172 lcd->printf("Sorting Paused.");
dreamselec 27:2cb1bdb7ae3d 173 lcd->locate(1,0);
dreamselec 27:2cb1bdb7ae3d 174 lcd->printf("1: Start");
dreamselec 27:2cb1bdb7ae3d 175 int button = 0;
dreamselec 27:2cb1bdb7ae3d 176 i2cport->write_bit(1, 12);
dreamselec 27:2cb1bdb7ae3d 177 i2cport->write_bit(1, 15);
dreamselec 27:2cb1bdb7ae3d 178 while (currentState == Pause && currentMode == Normal) {
dreamselec 27:2cb1bdb7ae3d 179 button = readSwitches();
dreamselec 27:2cb1bdb7ae3d 180 if (button == 1) {
dreamselec 27:2cb1bdb7ae3d 181 pc.printf(":<mbed>sort=start;");
dreamselec 27:2cb1bdb7ae3d 182 currentState = Start;
dreamselec 27:2cb1bdb7ae3d 183 } else if (button == 4) {
dreamselec 27:2cb1bdb7ae3d 184 currentMode = None;
dreamselec 27:2cb1bdb7ae3d 185 pc.printf(":<mbed>mode=none;");
dreamselec 27:2cb1bdb7ae3d 186 // goto setModeNone;
dreamselec 27:2cb1bdb7ae3d 187 break;
dreamselec 27:2cb1bdb7ae3d 188 }
dreamselec 27:2cb1bdb7ae3d 189 }
dreamselec 27:2cb1bdb7ae3d 190 }
dreamselec 27:2cb1bdb7ae3d 191 if (currentState == Start) {
dreamselec 27:2cb1bdb7ae3d 192 lcd->cls();
dreamselec 27:2cb1bdb7ae3d 193 lcd->locate(0,0);
dreamselec 27:2cb1bdb7ae3d 194 lcd->printf("Sorting mode...");
dreamselec 27:2cb1bdb7ae3d 195 while (currentState == Start && currentMode == Normal) {
dreamselec 27:2cb1bdb7ae3d 196 if (waitForBlock() == false) {
dreamselec 28:7e4d29977d72 197 if (connectedToPC == true && currentState != Pause && currentMode == Normal) {
dreamselec 27:2cb1bdb7ae3d 198 // TODO: Tell PC to update UI if aborted from MBED.
dreamselec 27:2cb1bdb7ae3d 199 pc.printf(":<mbed>sort=pause;");
dreamselec 27:2cb1bdb7ae3d 200 currentState = Pause;
dreamselec 27:2cb1bdb7ae3d 201 continue;
dreamselec 28:7e4d29977d72 202 }else if (connectedToPC == true && currentMode == None) {
dreamselec 28:7e4d29977d72 203 goto setModeNone;
dreamselec 28:7e4d29977d72 204 }else if (connectedToPC == false) {
dreamselec 27:2cb1bdb7ae3d 205 currentMode = None;
dreamselec 28:7e4d29977d72 206 currentState = Pause;
dreamselec 27:2cb1bdb7ae3d 207 abortOperation = true;
dreamselec 27:2cb1bdb7ae3d 208 }
dreamselec 27:2cb1bdb7ae3d 209 } else {
dreamselec 27:2cb1bdb7ae3d 210 sortBlock();
dreamselec 27:2cb1bdb7ae3d 211 }
dreamselec 27:2cb1bdb7ae3d 212 }
dreamselec 27:2cb1bdb7ae3d 213 }
dreamselec 27:2cb1bdb7ae3d 214 }
dreamselec 27:2cb1bdb7ae3d 215 } else if (currentMode == None) {
dreamselec 27:2cb1bdb7ae3d 216 setModeNone:
dreamselec 27:2cb1bdb7ae3d 217 D_LEDS_OFF();
dreamselec 27:2cb1bdb7ae3d 218 i2cport->write_bit(1,15);
dreamselec 27:2cb1bdb7ae3d 219 displayPCStatus();
dreamselec 27:2cb1bdb7ae3d 220 while (currentMode == None && abortOperation == false && connectedToPC == true) {
dreamselec 27:2cb1bdb7ae3d 221 if (i2cport->read_bit(11)) {
dreamselec 27:2cb1bdb7ae3d 222 abortOperation = displayAbortDialog();
dreamselec 27:2cb1bdb7ae3d 223 // Cancel the Abort
dreamselec 27:2cb1bdb7ae3d 224 if (abortOperation == false) {
dreamselec 27:2cb1bdb7ae3d 225 displayPCStatus();
dreamselec 27:2cb1bdb7ae3d 226 i2cport->write_bit(1, 15);
dreamselec 27:2cb1bdb7ae3d 227 }
dreamselec 27:2cb1bdb7ae3d 228 }
dreamselec 27:2cb1bdb7ae3d 229 }
dreamselec 27:2cb1bdb7ae3d 230 }
dreamselec 27:2cb1bdb7ae3d 231 }
dreamselec 15:777390eb5afd 232
dreamselec 27:2cb1bdb7ae3d 233 if (abortOperation == true ) {
dreamselec 27:2cb1bdb7ae3d 234 connectedToPC = false;
dreamselec 27:2cb1bdb7ae3d 235 D_LEDS_OFF();
dreamselec 27:2cb1bdb7ae3d 236 break;
dreamselec 27:2cb1bdb7ae3d 237 }
dreamselec 27:2cb1bdb7ae3d 238 }
dreamselec 27:2cb1bdb7ae3d 239 }
dreamselec 19:61b21ac4896e 240
dreamselec 27:2cb1bdb7ae3d 241 }
dreamselec 18:44a1c1a30166 242 }
dreamselec 8:e1da2ae62885 243
dreamselec 18:44a1c1a30166 244 // Waits until detects block.
dreamselec 18:44a1c1a30166 245 // true if block detected, false if cancelled/connected to PC.
dreamselec 19:61b21ac4896e 246 bool waitForBlock()
dreamselec 19:61b21ac4896e 247 {
dreamselec 27:2cb1bdb7ae3d 248 myLED4 = 0;
dreamselec 27:2cb1bdb7ae3d 249 myLED1 = 1;
dreamselec 27:2cb1bdb7ae3d 250 if (connectedToPC == false) {
dreamselec 27:2cb1bdb7ae3d 251 bool abortOperation = false;
dreamselec 27:2cb1bdb7ae3d 252 int blockInserted = 0;
dreamselec 27:2cb1bdb7ae3d 253 // Wait until a block is breaking the beam, or button 4 is pressed to abort.
dreamselec 27:2cb1bdb7ae3d 254 do {
dreamselec 27:2cb1bdb7ae3d 255 blockInserted = fpga->getBeamValue(Top);
dreamselec 27:2cb1bdb7ae3d 256 myLED4 = blockInserted;
dreamselec 27:2cb1bdb7ae3d 257 if (i2cport->read_bit(11)) {
dreamselec 27:2cb1bdb7ae3d 258 abortOperation = displayAbortDialog();
dreamselec 27:2cb1bdb7ae3d 259 // Cancel the Abort
dreamselec 27:2cb1bdb7ae3d 260 if (abortOperation == false) {
dreamselec 27:2cb1bdb7ae3d 261 displayWaitingLine();
dreamselec 27:2cb1bdb7ae3d 262 }
dreamselec 27:2cb1bdb7ae3d 263 }
dreamselec 27:2cb1bdb7ae3d 264 } while (abortOperation == false && blockInserted != 1 && connectedToPC == false);
dreamselec 18:44a1c1a30166 265
dreamselec 27:2cb1bdb7ae3d 266 if (abortOperation == true || connectedToPC == true)
dreamselec 27:2cb1bdb7ae3d 267 return false;
dreamselec 27:2cb1bdb7ae3d 268 else
dreamselec 27:2cb1bdb7ae3d 269 return true;
dreamselec 27:2cb1bdb7ae3d 270 } else if (connectedToPC == true) {
dreamselec 27:2cb1bdb7ae3d 271 bool abortOperation = false;
dreamselec 27:2cb1bdb7ae3d 272 int blockInserted = 0;
dreamselec 27:2cb1bdb7ae3d 273 // Wait until a block is breaking the beam, or button 4 is pressed to abort.
dreamselec 28:7e4d29977d72 274 pc.printf("INFO: Waiting for block.\n");
dreamselec 27:2cb1bdb7ae3d 275 do {
dreamselec 27:2cb1bdb7ae3d 276 blockInserted = fpga->getBeamValue(Top);
dreamselec 27:2cb1bdb7ae3d 277 myLED4 = blockInserted;
dreamselec 28:7e4d29977d72 278 if (readSwitches() == 4) {
dreamselec 27:2cb1bdb7ae3d 279 abortOperation = displayAbortDialog();
dreamselec 27:2cb1bdb7ae3d 280 // Cancel the Abort
dreamselec 27:2cb1bdb7ae3d 281 if (abortOperation == false) {
dreamselec 27:2cb1bdb7ae3d 282 displayWaitingLine();
dreamselec 27:2cb1bdb7ae3d 283 lcd->printf("for block");
dreamselec 27:2cb1bdb7ae3d 284 i2cport->write_bit(1, 15);
dreamselec 27:2cb1bdb7ae3d 285 }
dreamselec 27:2cb1bdb7ae3d 286 }
dreamselec 27:2cb1bdb7ae3d 287 } while (abortOperation == false && blockInserted != 1 && connectedToPC == true && currentState == Start && currentMode == Normal);
dreamselec 19:61b21ac4896e 288
dreamselec 28:7e4d29977d72 289 if (abortOperation == true || connectedToPC == false || currentState == Pause || currentMode != Normal)
dreamselec 27:2cb1bdb7ae3d 290 return false;
dreamselec 27:2cb1bdb7ae3d 291 else
dreamselec 27:2cb1bdb7ae3d 292 return true;
dreamselec 27:2cb1bdb7ae3d 293 }
dreamselec 27:2cb1bdb7ae3d 294 return false;
dreamselec 18:44a1c1a30166 295 }
dreamselec 14:cf2f255b5560 296
dreamselec 19:61b21ac4896e 297 void sortBlock()
dreamselec 19:61b21ac4896e 298 {
dreamselec 28:7e4d29977d72 299 pc.printf("BLOCK: Sorting block");
dreamselec 27:2cb1bdb7ae3d 300 myLED1 = 0;
dreamselec 27:2cb1bdb7ae3d 301 myLED2 = 1;
dreamselec 27:2cb1bdb7ae3d 302 // Cannot Abort any longer. Block is inserted.
dreamselec 27:2cb1bdb7ae3d 303 // Detach rx interrupt until block processed.
dreamselec 27:2cb1bdb7ae3d 304 NVIC_DisableIRQ(UART1_IRQn);
dreamselec 27:2cb1bdb7ae3d 305 fpga->moveSortingServo(Haz);
dreamselec 27:2cb1bdb7ae3d 306 fpga->moveStoppingServo(Go);
dreamselec 21:60c4fe94d79c 307
dreamselec 27:2cb1bdb7ae3d 308 int colourValues[4];
dreamselec 27:2cb1bdb7ae3d 309 int averageColourValues[4] = {0, 0, 0, 0};
dreamselec 27:2cb1bdb7ae3d 310 int numberOfReadings = 3;
dreamselec 27:2cb1bdb7ae3d 311 for (int i = 0; i < 4; i++) {
dreamselec 27:2cb1bdb7ae3d 312 rgbSensor.getAllColors(colourValues);
dreamselec 27:2cb1bdb7ae3d 313 for (int j = 0; j < 4; j++) {
dreamselec 27:2cb1bdb7ae3d 314 averageColourValues[j] += colourValues[j];
dreamselec 27:2cb1bdb7ae3d 315 }
dreamselec 27:2cb1bdb7ae3d 316 }
dreamselec 27:2cb1bdb7ae3d 317 for (int i = 0; i < 4; i++) {
dreamselec 27:2cb1bdb7ae3d 318 averageColourValues[i] = averageColourValues[i] / numberOfReadings;
dreamselec 27:2cb1bdb7ae3d 319 }
dreamselec 18:44a1c1a30166 320
dreamselec 27:2cb1bdb7ae3d 321 lastBlockHaz = false;
dreamselec 27:2cb1bdb7ae3d 322 lastBlockHaz = checkColour(averageColourValues);
dreamselec 28:7e4d29977d72 323 // lastBlockHaz = checkColour(colourValues);
dreamselec 27:2cb1bdb7ae3d 324
dreamselec 27:2cb1bdb7ae3d 325 if (!lastBlockHaz) {
dreamselec 27:2cb1bdb7ae3d 326 fpga->moveSortingServo(NonHaz);
dreamselec 27:2cb1bdb7ae3d 327 }
dreamselec 23:db91aaa43a9e 328
dreamselec 27:2cb1bdb7ae3d 329 myLED3 = 1;
dreamselec 27:2cb1bdb7ae3d 330 int blockSize;
dreamselec 27:2cb1bdb7ae3d 331 while (fpga->checkForBlock() == 0) { }
dreamselec 27:2cb1bdb7ae3d 332 blockSize = fpga->checkForSize();
dreamselec 27:2cb1bdb7ae3d 333 if (blockSize == HazBlock->size && lastBlockHaz) {
dreamselec 27:2cb1bdb7ae3d 334 // fpga->moveSortingServo(Haz);
dreamselec 27:2cb1bdb7ae3d 335 // fpga->moveStoppingServo(Go);
dreamselec 27:2cb1bdb7ae3d 336 // blockSize = HazBlock->size;
dreamselec 27:2cb1bdb7ae3d 337 while(fpga->getBeamValue(Bottom) == 1) {}
dreamselec 27:2cb1bdb7ae3d 338 wait(kServoWait);
dreamselec 27:2cb1bdb7ae3d 339 fpga->moveStoppingServo(Stop);
dreamselec 27:2cb1bdb7ae3d 340 } else {
dreamselec 27:2cb1bdb7ae3d 341 lastBlockHaz = false;
dreamselec 27:2cb1bdb7ae3d 342 }
dreamselec 27:2cb1bdb7ae3d 343 fpga->moveSortingServo(NonHaz);
dreamselec 27:2cb1bdb7ae3d 344 while(fpga->checkForSize()) {}
dreamselec 20:4e0f0944f28f 345
dreamselec 27:2cb1bdb7ae3d 346 if (connectedToPC) {
dreamselec 27:2cb1bdb7ae3d 347 for (int i = 0; i < 3; i++) {
dreamselec 29:9c0339e3c593 348 pc.printf("DEBUG:Percentage Error: %.5f. Passed?: %i\n", percentageError[i], isBetweenHazValues[i]);
dreamselec 29:9c0339e3c593 349 // if ((percentageError[i] < 0 && std::abs(percentageError[i]) < kMinRedError[i] * errorMultiplier) || percentageError[i] == 0 || (percentageError[i] > 0 && percentageError[i] < kMaxRedError[i] * errorMultiplier))
dreamselec 29:9c0339e3c593 350 // pc.printf("DEBUG:%i Pass.\n", i);
dreamselec 27:2cb1bdb7ae3d 351 }
dreamselec 27:2cb1bdb7ae3d 352 pc.printf("BLOCK:Size:%i,Red:%.3f,Green:%.3f,Blue:%.3f,Haz:%i, Offsetred:%.3f, Offsetgreen:%.3f, Offsetblue:%.3f;", blockSize, adjustedValues[0], adjustedValues[1], adjustedValues[2], lastBlockHaz, percentageError[0], percentageError[1], percentageError[2]);
dreamselec 27:2cb1bdb7ae3d 353 // pc.printf("VALUE:Size:%i,Red:%i,Green:%i,Blue:%i,Clear:%i\n:VALUE", blockSize, colourValues[0], colourValues[1], colourValues[2], colourValues[3], lastBlockHaz);
dreamselec 27:2cb1bdb7ae3d 354 }
dreamselec 18:44a1c1a30166 355
dreamselec 27:2cb1bdb7ae3d 356 // Re-Attach rx interrupt
dreamselec 27:2cb1bdb7ae3d 357 NVIC_EnableIRQ(UART1_IRQn);
dreamselec 27:2cb1bdb7ae3d 358
dreamselec 27:2cb1bdb7ae3d 359 blockCount++;
dreamselec 27:2cb1bdb7ae3d 360 myLED3 = 0;
dreamselec 27:2cb1bdb7ae3d 361 myLED4 = 1;
dreamselec 27:2cb1bdb7ae3d 362 return;
dreamselec 6:98fe30430194 363 }
dreamselec 6:98fe30430194 364
dreamselec 6:98fe30430194 365 /// Called every-time it receives an char from PC.
dreamselec 10:16ba52f8e025 366 void Rx_interrupt()
dreamselec 10:16ba52f8e025 367 {
dreamselec 28:7e4d29977d72 368 recievingResponse = true;
dreamselec 27:2cb1bdb7ae3d 369 char interruptChar = pc.getc();
dreamselec 27:2cb1bdb7ae3d 370 // Uncomment to Echo to USB serial to watch data flow
dreamselec 27:2cb1bdb7ae3d 371 // pc.putc(interruptChar);
dreamselec 6:98fe30430194 372
dreamselec 27:2cb1bdb7ae3d 373 NVIC_DisableIRQ(UART1_IRQn);
dreamselec 8:e1da2ae62885 374
dreamselec 27:2cb1bdb7ae3d 375 if (interruptChar == CommandTypeValue[Query]) {
dreamselec 27:2cb1bdb7ae3d 376 commander->decodeCommand(Query);
dreamselec 27:2cb1bdb7ae3d 377 } else if (interruptChar == CommandTypeValue[Set]) {
dreamselec 27:2cb1bdb7ae3d 378 commander->decodeCommand(Set);
dreamselec 27:2cb1bdb7ae3d 379 } else if (interruptChar== CommandTypeValue[Reply]) {
dreamselec 27:2cb1bdb7ae3d 380 commander->decodeCommand(Reply);
dreamselec 27:2cb1bdb7ae3d 381 }
dreamselec 8:e1da2ae62885 382
dreamselec 27:2cb1bdb7ae3d 383 NVIC_EnableIRQ(UART1_IRQn);
dreamselec 28:7e4d29977d72 384 recievingResponse = false;
dreamselec 6:98fe30430194 385 }
dreamselec 3:843b830ee8bd 386
dreamselec 4:4eebb4de22a7 387 void initInternal()
dreamselec 4:4eebb4de22a7 388 {
dreamselec 27:2cb1bdb7ae3d 389 myLED1 = 1;
dreamselec 27:2cb1bdb7ae3d 390 i2cport = new MCP23017(p9, p10, 0x40);
dreamselec 27:2cb1bdb7ae3d 391 lcd = new WattBob_TextLCD(i2cport);
dreamselec 27:2cb1bdb7ae3d 392 BACKLIGHT_ON(i2cport);
dreamselec 27:2cb1bdb7ae3d 393 lcd->cls();
dreamselec 27:2cb1bdb7ae3d 394 lcd->locate(0,0);
dreamselec 27:2cb1bdb7ae3d 395 lcd->printf("Initilizing...");
dreamselec 29:9c0339e3c593 396 // DefaultHazBlock();
dreamselec 28:7e4d29977d72 397 rgbSensor.enablePowerAndRGBC();
dreamselec 28:7e4d29977d72 398 rgbSensor.setIntegrationTime(gIntegrationTime);
dreamselec 28:7e4d29977d72 399 srand((unsigned)time(NULL));
dreamselec 27:2cb1bdb7ae3d 400 myLED2 = 1;
dreamselec 28:7e4d29977d72 401 fpga->moveStoppingServo(Stop);
dreamselec 28:7e4d29977d72 402 fpga->moveSortingServo(NonHaz);
dreamselec 27:2cb1bdb7ae3d 403 return;
dreamselec 3:843b830ee8bd 404 }
dreamselec 3:843b830ee8bd 405
dreamselec 7:b6e31bfdb2af 406 void initPort(int baudRate)
dreamselec 4:4eebb4de22a7 407 {
dreamselec 27:2cb1bdb7ae3d 408 myLED3 = 1;
dreamselec 27:2cb1bdb7ae3d 409 pc.baud(baudRate);
dreamselec 27:2cb1bdb7ae3d 410 pc.format(8, SerialBase::None, gStopBits);
dreamselec 28:7e4d29977d72 411 pc.attach(&Rx_interrupt, Serial::RxIrq);
dreamselec 28:7e4d29977d72 412 myLED4 = 1;
dreamselec 28:7e4d29977d72 413 pc.printf(":<pc>connect;");
dreamselec 28:7e4d29977d72 414 // wait (0.3);
dreamselec 28:7e4d29977d72 415 // while(recievingResponse == true) {}
dreamselec 28:7e4d29977d72 416 turnOffTopLEDs();
dreamselec 27:2cb1bdb7ae3d 417 return;
dreamselec 3:843b830ee8bd 418 }
dreamselec 3:843b830ee8bd 419
dreamselec 18:44a1c1a30166 420 bool checkColour(int colourValues[])
dreamselec 15:777390eb5afd 421 {
dreamselec 30:c0bc92d009fe 422 for (int i = 0; i < 3; i++){
dreamselec 30:c0bc92d009fe 423 isBetweenHazValues[i] = false;
dreamselec 30:c0bc92d009fe 424 }
dreamselec 27:2cb1bdb7ae3d 425 memset(adjustedValues, 0, sizeof(adjustedValues));
dreamselec 27:2cb1bdb7ae3d 426 memset(percentageError, 0, sizeof(percentageError));
dreamselec 27:2cb1bdb7ae3d 427
dreamselec 27:2cb1bdb7ae3d 428 /* Harcoded working
dreamselec 27:2cb1bdb7ae3d 429 for (int i = 0; i < 3; i++) {
dreamselec 27:2cb1bdb7ae3d 430 adjustedValues[i] = (float)colourValues[i]/(float)colourValues[3];
dreamselec 27:2cb1bdb7ae3d 431 percentageError[i] = (adjustedValues[i] - kAverageRedBlock[i]) / kAverageRedBlock[i];
dreamselec 15:777390eb5afd 432
dreamselec 27:2cb1bdb7ae3d 433 if ((percentageError[i] < 0 && std::abs(percentageError[i]) < kMinError[i] * 2) || percentageError[i] == 0 || (percentageError[i] > 0 && percentageError[i] < kMaxError[i] * 2)) {
dreamselec 29:9c0339e3c593 434 isBetweenHazValues[i] = true;
dreamselec 27:2cb1bdb7ae3d 435 }
dreamselec 27:2cb1bdb7ae3d 436 }
dreamselec 27:2cb1bdb7ae3d 437 */
dreamselec 29:9c0339e3c593 438 /*
dreamselec 27:2cb1bdb7ae3d 439 for (int i = 0; i < 3; i++) {
dreamselec 27:2cb1bdb7ae3d 440 adjustedValues[i] = (float)colourValues[i]/(float)colourValues[3];
dreamselec 27:2cb1bdb7ae3d 441 percentageError[i] = (adjustedValues[i] - kAverageRedBlock[i]) / kAverageRedBlock[i];
dreamselec 29:9c0339e3c593 442 // Don't forget to call DefaultHazBlock in init otherwise currentMin/MaxErr won't be set.
dreamselec 27:2cb1bdb7ae3d 443 if ((percentageError[i] < 0 && std::abs(percentageError[i]) < currentMinError[i] * errorMultiplier) || percentageError[i] == 0 || (percentageError[i] > 0 && percentageError[i] < currentMaxError[i] * errorMultiplier)) {
dreamselec 27:2cb1bdb7ae3d 444 isHazColour[i] = true;
dreamselec 27:2cb1bdb7ae3d 445 }
dreamselec 27:2cb1bdb7ae3d 446 }
dreamselec 29:9c0339e3c593 447 */
dreamselec 29:9c0339e3c593 448 for (int i = 0; i < 3; i++) {
dreamselec 29:9c0339e3c593 449 adjustedValues[i] = (float)colourValues[i]/(float)colourValues[3];
dreamselec 29:9c0339e3c593 450 percentageError[i] = (adjustedValues[i] - kAverageValues[HazBlock->colour][i]) / kAverageValues[HazBlock->colour][i];
dreamselec 15:777390eb5afd 451
dreamselec 29:9c0339e3c593 452 if ((percentageError[i] < 0 && std::abs(percentageError[i]) < kMinError[HazBlock->colour][i] * errorMultiplier) || percentageError[i] == 0 || (percentageError[i] > 0 && percentageError[i] < kMaxError[HazBlock->colour][i] * errorMultiplier)) {
dreamselec 29:9c0339e3c593 453 isBetweenHazValues[i] = true;
dreamselec 29:9c0339e3c593 454 }
dreamselec 29:9c0339e3c593 455 }
dreamselec 27:2cb1bdb7ae3d 456
dreamselec 27:2cb1bdb7ae3d 457 bool isHazBlock = false;
dreamselec 15:777390eb5afd 458
dreamselec 29:9c0339e3c593 459 if (isBetweenHazValues[0] && isBetweenHazValues[1] && isBetweenHazValues[2]) {
dreamselec 27:2cb1bdb7ae3d 460 isHazBlock = true;
dreamselec 27:2cb1bdb7ae3d 461 } else {
dreamselec 27:2cb1bdb7ae3d 462 isHazBlock = false;
dreamselec 27:2cb1bdb7ae3d 463 }
dreamselec 27:2cb1bdb7ae3d 464 myLED2 = 0;
dreamselec 27:2cb1bdb7ae3d 465 return isHazBlock;
dreamselec 3:843b830ee8bd 466 }
dreamselec 3:843b830ee8bd 467
dreamselec 30:c0bc92d009fe 468 void newHazBlockMode(){
dreamselec 30:c0bc92d009fe 469 trySetHazBlockAgain:
dreamselec 30:c0bc92d009fe 470 pc.printf("NHZB:Size:%i,Colour:%i;", _HazBlock.size, _HazBlock.colour);
dreamselec 30:c0bc92d009fe 471 fpga->moveSortingServo(Haz);
dreamselec 30:c0bc92d009fe 472 // pc.printf("INFO:Setting new haz block.\n");
dreamselec 30:c0bc92d009fe 473 int lowerBeam = 0;
dreamselec 30:c0bc92d009fe 474 int higherBeam = 0;
dreamselec 30:c0bc92d009fe 475 int colourValues[6][4];
dreamselec 30:c0bc92d009fe 476 for (int i = 0; i < 6; i++){
dreamselec 30:c0bc92d009fe 477 memset(colourValues[i], 0, sizeof(colourValues[6]));
dreamselec 30:c0bc92d009fe 478 }
dreamselec 30:c0bc92d009fe 479 int readingCount = 0;
dreamselec 30:c0bc92d009fe 480
dreamselec 30:c0bc92d009fe 481 lcd->cls();
dreamselec 30:c0bc92d009fe 482 lcd->locate(0,0);
dreamselec 30:c0bc92d009fe 483 lcd->printf("New haz block");
dreamselec 30:c0bc92d009fe 484
dreamselec 30:c0bc92d009fe 485 do {
dreamselec 30:c0bc92d009fe 486 higherBeam = fpga->getBeamValue(Top);
dreamselec 30:c0bc92d009fe 487 if (readSwitches() == 4) {
dreamselec 30:c0bc92d009fe 488 if (displayAbortDialog()) {
dreamselec 30:c0bc92d009fe 489 //TODO: tell pc
dreamselec 30:c0bc92d009fe 490 pc.printf(":<mbed>haz-block=pause;");
dreamselec 30:c0bc92d009fe 491 pc.printf("INFO: Operation aborted form MBED.\n");
dreamselec 30:c0bc92d009fe 492 fpga->moveSortingServo(NonHaz);
dreamselec 30:c0bc92d009fe 493 displayPCStatus();
dreamselec 30:c0bc92d009fe 494 return;
dreamselec 30:c0bc92d009fe 495 } else {
dreamselec 30:c0bc92d009fe 496 lcd->cls();
dreamselec 30:c0bc92d009fe 497 lcd->locate(0,0);
dreamselec 30:c0bc92d009fe 498 lcd->printf("New haz block");
dreamselec 30:c0bc92d009fe 499 }
dreamselec 30:c0bc92d009fe 500 }
dreamselec 30:c0bc92d009fe 501 } while (higherBeam != 1 && setNewHazBlock == true);
dreamselec 30:c0bc92d009fe 502
dreamselec 30:c0bc92d009fe 503 if (setNewHazBlock == false) {
dreamselec 30:c0bc92d009fe 504 displayPCStatus();
dreamselec 30:c0bc92d009fe 505 return;
dreamselec 30:c0bc92d009fe 506 }
dreamselec 30:c0bc92d009fe 507
dreamselec 30:c0bc92d009fe 508 do {
dreamselec 30:c0bc92d009fe 509 rgbSensor.getAllColors(colourValues[readingCount]);
dreamselec 30:c0bc92d009fe 510 readingCount++;
dreamselec 30:c0bc92d009fe 511 } while (readingCount < 6 && fpga->getBeamValue(Top) == 1 && fpga->getBeamValue(Bottom) == 0);
dreamselec 30:c0bc92d009fe 512
dreamselec 30:c0bc92d009fe 513 lowerBeam = fpga->getBeamValue(Bottom);
dreamselec 30:c0bc92d009fe 514 higherBeam = fpga->getBeamValue(Top);
dreamselec 30:c0bc92d009fe 515 if (lowerBeam != 1){
dreamselec 30:c0bc92d009fe 516 lowerBeam = fpga->getBeamValue(Top);
dreamselec 30:c0bc92d009fe 517 while(lowerBeam != 1) { lowerBeam = fpga->getBeamValue(Bottom); }
dreamselec 30:c0bc92d009fe 518 higherBeam = fpga->getBeamValue(Bottom);
dreamselec 30:c0bc92d009fe 519 }
dreamselec 30:c0bc92d009fe 520 int blockSize = higherBeam;
dreamselec 30:c0bc92d009fe 521
dreamselec 30:c0bc92d009fe 522 int totalComponents[4];
dreamselec 30:c0bc92d009fe 523 memset(totalComponents, 0, sizeof(totalComponents));
dreamselec 30:c0bc92d009fe 524 for (int k = 0; k < readingCount; k++){
dreamselec 30:c0bc92d009fe 525 for (int i = 0; i < 4; i++){
dreamselec 30:c0bc92d009fe 526 totalComponents[i] += colourValues[k][i];
dreamselec 30:c0bc92d009fe 527 }
dreamselec 30:c0bc92d009fe 528 }
dreamselec 30:c0bc92d009fe 529 float averageComponents[4];
dreamselec 30:c0bc92d009fe 530 memset(averageComponents, 0, sizeof(averageComponents));
dreamselec 30:c0bc92d009fe 531 for (int i = 0; i < 4; i++){
dreamselec 30:c0bc92d009fe 532 averageComponents[i] = ((float)totalComponents[i] / (float)readingCount);
dreamselec 30:c0bc92d009fe 533 }
dreamselec 30:c0bc92d009fe 534 float adjustedValues[3];
dreamselec 30:c0bc92d009fe 535 memset(adjustedValues, 0, sizeof(adjustedValues));
dreamselec 30:c0bc92d009fe 536 for (int i = 0; i < 3; i++){
dreamselec 30:c0bc92d009fe 537 adjustedValues[i] = averageComponents[i] / averageComponents[3];
dreamselec 30:c0bc92d009fe 538 }
dreamselec 30:c0bc92d009fe 539
dreamselec 30:c0bc92d009fe 540 Block::BlockColour detectedColour = Block::Wrong;
dreamselec 30:c0bc92d009fe 541 bool matchesColour[3] = {false, false, false};
dreamselec 30:c0bc92d009fe 542
dreamselec 30:c0bc92d009fe 543 float percentageError[3];
dreamselec 30:c0bc92d009fe 544 memset(percentageError, 0, sizeof(percentageError));
dreamselec 30:c0bc92d009fe 545 for (int k = 0; k < 7; k++){
dreamselec 30:c0bc92d009fe 546 for (int i = 0; i < 3; i++) {
dreamselec 30:c0bc92d009fe 547 percentageError[i] = (adjustedValues[i] - kAverageValues[_HazBlock.colour][i]) / kAverageValues[_HazBlock.colour][i];
dreamselec 30:c0bc92d009fe 548 pc.printf("DEBUG: Percenage error: %.5f\n", percentageError[i]);
dreamselec 30:c0bc92d009fe 549
dreamselec 30:c0bc92d009fe 550 if ((percentageError[i] < 0 && std::abs(percentageError[i]) < kMinError[_HazBlock.colour][i] * errorMultiplier) || percentageError[i] == 0 || (percentageError[i] > 0 && percentageError[i] < kMaxError[_HazBlock.colour][i] * errorMultiplier)) {
dreamselec 30:c0bc92d009fe 551 pc.printf("DEBUG: Pass\n");
dreamselec 30:c0bc92d009fe 552 matchesColour[i] = true;
dreamselec 30:c0bc92d009fe 553 }
dreamselec 30:c0bc92d009fe 554 if (matchesColour[0] && matchesColour[1] && matchesColour[2]){
dreamselec 30:c0bc92d009fe 555 detectedColour = static_cast<Block::BlockColour>(k);
dreamselec 30:c0bc92d009fe 556 break;
dreamselec 30:c0bc92d009fe 557 }
dreamselec 30:c0bc92d009fe 558 }
dreamselec 30:c0bc92d009fe 559 }
dreamselec 30:c0bc92d009fe 560
dreamselec 30:c0bc92d009fe 561 if (detectedColour != Block::Wrong){
dreamselec 30:c0bc92d009fe 562 pc.printf("ERROR: Could not detect colour.\n");
dreamselec 30:c0bc92d009fe 563 lcd->cls();
dreamselec 30:c0bc92d009fe 564 lcd->printf("1: Try again");
dreamselec 30:c0bc92d009fe 565 lcd->locate(1,0);
dreamselec 30:c0bc92d009fe 566 lcd->printf("2: Revert to last");
dreamselec 30:c0bc92d009fe 567 int button = 0;
dreamselec 30:c0bc92d009fe 568 do {
dreamselec 30:c0bc92d009fe 569 button = readSwitches();
dreamselec 30:c0bc92d009fe 570 if (button == 1){
dreamselec 30:c0bc92d009fe 571 goto trySetHazBlockAgain;
dreamselec 30:c0bc92d009fe 572 }
dreamselec 30:c0bc92d009fe 573 } while (button != 2);
dreamselec 30:c0bc92d009fe 574 }
dreamselec 30:c0bc92d009fe 575
dreamselec 30:c0bc92d009fe 576 // Point and literal might not sync...
dreamselec 30:c0bc92d009fe 577 _HazBlock.size = static_cast<Block::Size>(blockSize);
dreamselec 30:c0bc92d009fe 578 _HazBlock.colour = detectedColour;
dreamselec 30:c0bc92d009fe 579
dreamselec 30:c0bc92d009fe 580 pc.printf("NHZB:Size:%i,Colour:%i;", _HazBlock.size, _HazBlock.colour);
dreamselec 30:c0bc92d009fe 581
dreamselec 30:c0bc92d009fe 582
dreamselec 30:c0bc92d009fe 583 pc.printf("VALUE:Hazardous Block:\n \tSize:%i \n \tMin Error:%i, %i, %i\n \t Max Error:%i, %i, %i\n:VALUE", _HazBlock.size, kMinError[_HazBlock.colour][1], kMinError[_HazBlock.colour][1], kMinError[_HazBlock.colour][2], kMaxError[_HazBlock.colour][0], kMaxError[_HazBlock.colour][1], kMaxError[_HazBlock.colour][2]);
dreamselec 30:c0bc92d009fe 584 pc.printf("VALUE:\tAverage Colour:%.3f, %.3f, %.3f, %.3f\n:VALUE", kAverageValues[_HazBlock.colour][0], kAverageValues[_HazBlock.colour][1], kAverageValues[_HazBlock.colour][2], kAverageValues[_HazBlock.colour][3]);
dreamselec 30:c0bc92d009fe 585 fpga->moveSortingServo(NonHaz);
dreamselec 30:c0bc92d009fe 586 }
dreamselec 30:c0bc92d009fe 587
dreamselec 6:98fe30430194 588 void printPCDetectedText()
dreamselec 4:4eebb4de22a7 589 {
dreamselec 27:2cb1bdb7ae3d 590 lcd->cls();
dreamselec 27:2cb1bdb7ae3d 591 LCDFL();
dreamselec 27:2cb1bdb7ae3d 592 lcd->printf("Detected PC.");
dreamselec 27:2cb1bdb7ae3d 593 LCDSL();
dreamselec 27:2cb1bdb7ae3d 594 lcd->printf("Connecting");
dreamselec 27:2cb1bdb7ae3d 595 initPort();
dreamselec 10:16ba52f8e025 596 }
dreamselec 10:16ba52f8e025 597
dreamselec 10:16ba52f8e025 598 bool displayAbortDialog()
dreamselec 10:16ba52f8e025 599 {
dreamselec 27:2cb1bdb7ae3d 600 while (i2cport->read_bit(11) == 1) {}
dreamselec 27:2cb1bdb7ae3d 601 i2cport->write_bit(1, 12);
dreamselec 10:16ba52f8e025 602
dreamselec 27:2cb1bdb7ae3d 603 lcd->cls();
dreamselec 27:2cb1bdb7ae3d 604 LCDFL();
dreamselec 27:2cb1bdb7ae3d 605 lcd->printf("Abort?");
dreamselec 27:2cb1bdb7ae3d 606 LCDSL();
dreamselec 27:2cb1bdb7ae3d 607 lcd->printf("1:Yes, 2,3,4:No");
dreamselec 27:2cb1bdb7ae3d 608 int reply = 0;
dreamselec 27:2cb1bdb7ae3d 609 do {
dreamselec 27:2cb1bdb7ae3d 610 reply = readSwitches();
dreamselec 27:2cb1bdb7ae3d 611 } while(reply == 0);
dreamselec 10:16ba52f8e025 612
dreamselec 27:2cb1bdb7ae3d 613 D_LEDS_OFF();
dreamselec 27:2cb1bdb7ae3d 614 if (reply == 1) {
dreamselec 27:2cb1bdb7ae3d 615 // while (i2cport->read_bit(8)) { }
dreamselec 27:2cb1bdb7ae3d 616 return true;
dreamselec 27:2cb1bdb7ae3d 617 } else {
dreamselec 27:2cb1bdb7ae3d 618 // while (i2cport->read_bit(9) || i2cport->read_bit(10) || i2cport->read_bit(11)) { }
dreamselec 27:2cb1bdb7ae3d 619 return false;
dreamselec 27:2cb1bdb7ae3d 620 }
dreamselec 3:843b830ee8bd 621 }
dreamselec 3:843b830ee8bd 622
dreamselec 15:777390eb5afd 623 void printServoInfoOnLCD()
dreamselec 15:777390eb5afd 624 {
dreamselec 27:2cb1bdb7ae3d 625 lcd->cls();
dreamselec 27:2cb1bdb7ae3d 626 lcd->locate(0,0);
dreamselec 27:2cb1bdb7ae3d 627 if (fpga->stoppingServoPosition == Stop)
dreamselec 27:2cb1bdb7ae3d 628 lcd->printf("1:Top: Go");
dreamselec 27:2cb1bdb7ae3d 629 else if (fpga->stoppingServoPosition == Go)
dreamselec 27:2cb1bdb7ae3d 630 lcd->printf("1:Top: Stop");
dreamselec 23:db91aaa43a9e 631
dreamselec 27:2cb1bdb7ae3d 632 lcd->locate(1,0);
dreamselec 27:2cb1bdb7ae3d 633 if (fpga->sortingServoPosition == NonHaz)
dreamselec 27:2cb1bdb7ae3d 634 lcd->printf("2:Bottom: Haz");
dreamselec 27:2cb1bdb7ae3d 635 else if (fpga->sortingServoPosition == Haz)
dreamselec 27:2cb1bdb7ae3d 636 lcd->printf("2:Bottom: NonHaz");
dreamselec 23:db91aaa43a9e 637 }
dreamselec 23:db91aaa43a9e 638
dreamselec 27:2cb1bdb7ae3d 639 void printServoInfoOnPC()
dreamselec 27:2cb1bdb7ae3d 640 {
dreamselec 27:2cb1bdb7ae3d 641 if (fpga->stoppingServoPosition == Stop)
dreamselec 27:2cb1bdb7ae3d 642 pc.printf(":<servos>1=Stop;");
dreamselec 27:2cb1bdb7ae3d 643 else if (fpga->stoppingServoPosition == Go)
dreamselec 27:2cb1bdb7ae3d 644 pc.printf(":<servos>1=Go;");
dreamselec 23:db91aaa43a9e 645
dreamselec 27:2cb1bdb7ae3d 646 if (fpga->sortingServoPosition == NonHaz)
dreamselec 27:2cb1bdb7ae3d 647 pc.printf(":<servos>2=NonHaz;");
dreamselec 27:2cb1bdb7ae3d 648 else if (fpga->sortingServoPosition == Haz)
dreamselec 27:2cb1bdb7ae3d 649 pc.printf(":<servos>2=Haz;");
dreamselec 10:16ba52f8e025 650 }
dreamselec 10:16ba52f8e025 651
dreamselec 15:777390eb5afd 652 void runInServoTestMode()
dreamselec 15:777390eb5afd 653 {
dreamselec 27:2cb1bdb7ae3d 654 pc.printf("VALUES:Testing servos.\n Stopping servo:\n\tStop:%i, Go: %i\n Sorting servo:\n\tHaz:%i, NonHaz:%i\n:VALUES", Stop, Go, Haz, NonHaz);
dreamselec 27:2cb1bdb7ae3d 655 printServoInfoOnPC();
dreamselec 27:2cb1bdb7ae3d 656 printServoInfoOnLCD();
dreamselec 27:2cb1bdb7ae3d 657
dreamselec 27:2cb1bdb7ae3d 658 i2cport->write_bit(1, 12);
dreamselec 27:2cb1bdb7ae3d 659 i2cport->write_bit(1, 13);
dreamselec 27:2cb1bdb7ae3d 660 i2cport->write_bit(1, 15);
dreamselec 27:2cb1bdb7ae3d 661 int button = 0;
dreamselec 27:2cb1bdb7ae3d 662 bool finished = false;
dreamselec 27:2cb1bdb7ae3d 663 do {
dreamselec 27:2cb1bdb7ae3d 664 button = readSwitches();
dreamselec 27:2cb1bdb7ae3d 665
dreamselec 27:2cb1bdb7ae3d 666 // gToggleServoNumber: 1 = Toggle top servo, 2 = Toggle bottom servo, 3 = Toggle both servos
dreamselec 27:2cb1bdb7ae3d 667 if (gToggleServoNumber == 1) {
dreamselec 27:2cb1bdb7ae3d 668 button = 1;
dreamselec 27:2cb1bdb7ae3d 669 gToggleServoNumber = 0;
dreamselec 27:2cb1bdb7ae3d 670 } else if (gToggleServoNumber == 2) {
dreamselec 27:2cb1bdb7ae3d 671 button = 2;
dreamselec 27:2cb1bdb7ae3d 672 gToggleServoNumber = 0;
dreamselec 27:2cb1bdb7ae3d 673 }
dreamselec 23:db91aaa43a9e 674
dreamselec 27:2cb1bdb7ae3d 675 if (button == 1) {
dreamselec 27:2cb1bdb7ae3d 676 fpga->toggleStoppingServo();
dreamselec 27:2cb1bdb7ae3d 677 printServoInfoOnLCD();
dreamselec 27:2cb1bdb7ae3d 678 printServoInfoOnPC();
dreamselec 27:2cb1bdb7ae3d 679 wait(kServoWait);
dreamselec 27:2cb1bdb7ae3d 680 } else if (button == 2) {
dreamselec 27:2cb1bdb7ae3d 681 fpga->toggleSortingServo();
dreamselec 27:2cb1bdb7ae3d 682 printServoInfoOnLCD();
dreamselec 27:2cb1bdb7ae3d 683 printServoInfoOnPC();
dreamselec 27:2cb1bdb7ae3d 684 wait(kServoWait);
dreamselec 27:2cb1bdb7ae3d 685 }
dreamselec 23:db91aaa43a9e 686
dreamselec 27:2cb1bdb7ae3d 687 finished = button == 4;
dreamselec 27:2cb1bdb7ae3d 688 button = 0;
dreamselec 27:2cb1bdb7ae3d 689 } while (finished == false && runServoTest == true);
dreamselec 27:2cb1bdb7ae3d 690
dreamselec 27:2cb1bdb7ae3d 691 D_LEDS_OFF();
dreamselec 27:2cb1bdb7ae3d 692 lcd->cls();
dreamselec 27:2cb1bdb7ae3d 693 lcd->locate(0,0);
dreamselec 27:2cb1bdb7ae3d 694 lcd->printf("Done servo test");
dreamselec 28:7e4d29977d72 695 fpga->moveSortingServo(NonHaz);
dreamselec 27:2cb1bdb7ae3d 696 if (runServoTest == true) {
dreamselec 27:2cb1bdb7ae3d 697 pc.printf(":<servos>test=pause;");
dreamselec 27:2cb1bdb7ae3d 698 runServoTest = false;
dreamselec 27:2cb1bdb7ae3d 699 }
dreamselec 27:2cb1bdb7ae3d 700 wait(0.5);
dreamselec 27:2cb1bdb7ae3d 701 lcd->cls();
dreamselec 27:2cb1bdb7ae3d 702 return;
dreamselec 23:db91aaa43a9e 703 }
dreamselec 15:777390eb5afd 704
dreamselec 27:2cb1bdb7ae3d 705 void printBeamInfoOnLCD()
dreamselec 27:2cb1bdb7ae3d 706 {
dreamselec 27:2cb1bdb7ae3d 707 lcd->cls();
dreamselec 27:2cb1bdb7ae3d 708 lcd->locate(0,0);
dreamselec 27:2cb1bdb7ae3d 709 lcd->printf("Top:L1 Btm: L2");
dreamselec 27:2cb1bdb7ae3d 710 lcd->locate(1,0);
dreamselec 27:2cb1bdb7ae3d 711 lcd->printf("On:High, Off:Low");
dreamselec 23:db91aaa43a9e 712 }
dreamselec 23:db91aaa43a9e 713
dreamselec 27:2cb1bdb7ae3d 714 void printBeamInfoOnPC(int topBeam, int bottomBeam)
dreamselec 27:2cb1bdb7ae3d 715 {
dreamselec 27:2cb1bdb7ae3d 716 pc.printf(":<break_beam>2=%i,1=%i;", topBeam, bottomBeam);
dreamselec 23:db91aaa43a9e 717 }
dreamselec 23:db91aaa43a9e 718
dreamselec 27:2cb1bdb7ae3d 719 void runInBreakBeamTestMode()
dreamselec 27:2cb1bdb7ae3d 720 {
dreamselec 27:2cb1bdb7ae3d 721 turnOffTopLEDs();
dreamselec 27:2cb1bdb7ae3d 722 i2cport->write_bit(1, 15);
dreamselec 23:db91aaa43a9e 723
dreamselec 27:2cb1bdb7ae3d 724 int topBeamValue = fpga->getBeamValue(1);
dreamselec 27:2cb1bdb7ae3d 725 int bottomBeamValue = fpga->getBeamValue(2);
dreamselec 27:2cb1bdb7ae3d 726 printBeamInfoOnPC(topBeamValue, bottomBeamValue);
dreamselec 27:2cb1bdb7ae3d 727 printBeamInfoOnLCD();
dreamselec 27:2cb1bdb7ae3d 728
dreamselec 27:2cb1bdb7ae3d 729 int button = 0;
dreamselec 27:2cb1bdb7ae3d 730 bool finished = false;
dreamselec 27:2cb1bdb7ae3d 731 do {
dreamselec 27:2cb1bdb7ae3d 732 button = readSwitches();
dreamselec 27:2cb1bdb7ae3d 733 int currentTopBeamValue = fpga->getBeamValue(1);
dreamselec 27:2cb1bdb7ae3d 734 int currentBottomBeamValue = fpga->getBeamValue(2);
dreamselec 18:44a1c1a30166 735
dreamselec 27:2cb1bdb7ae3d 736 // For debugging, hold down both 1 and 3 or 2 and 3
dreamselec 27:2cb1bdb7ae3d 737 if (i2cport->read_bit(10) == 1) {
dreamselec 27:2cb1bdb7ae3d 738 currentTopBeamValue = i2cport->read_bit(8) && i2cport->read_bit(10);
dreamselec 27:2cb1bdb7ae3d 739 currentBottomBeamValue = i2cport->read_bit(9) && i2cport->read_bit(10);
dreamselec 27:2cb1bdb7ae3d 740 }
dreamselec 27:2cb1bdb7ae3d 741 myLED1 = currentTopBeamValue;
dreamselec 27:2cb1bdb7ae3d 742 myLED2 = currentBottomBeamValue;
dreamselec 23:db91aaa43a9e 743
dreamselec 27:2cb1bdb7ae3d 744 if (currentTopBeamValue != topBeamValue || currentBottomBeamValue != bottomBeamValue ) {
dreamselec 27:2cb1bdb7ae3d 745 topBeamValue = currentTopBeamValue;
dreamselec 27:2cb1bdb7ae3d 746 bottomBeamValue = currentBottomBeamValue;
dreamselec 27:2cb1bdb7ae3d 747 printBeamInfoOnPC(topBeamValue, bottomBeamValue);
dreamselec 27:2cb1bdb7ae3d 748 }
dreamselec 27:2cb1bdb7ae3d 749 finished = button == 4;
dreamselec 27:2cb1bdb7ae3d 750 } while (runBreakBeamTest == true && finished == false);
dreamselec 23:db91aaa43a9e 751
dreamselec 27:2cb1bdb7ae3d 752 turnOffBottomLEDs();
dreamselec 27:2cb1bdb7ae3d 753 lcd->cls();
dreamselec 27:2cb1bdb7ae3d 754 lcd->locate(0,0);
dreamselec 27:2cb1bdb7ae3d 755 lcd->printf("Finished test");
dreamselec 27:2cb1bdb7ae3d 756 if (runBreakBeamTest == true) {
dreamselec 27:2cb1bdb7ae3d 757 pc.printf(":<break_beam>test=pause;");
dreamselec 27:2cb1bdb7ae3d 758 runBreakBeamTest = false;
dreamselec 27:2cb1bdb7ae3d 759 }
dreamselec 27:2cb1bdb7ae3d 760 wait(0.5);
dreamselec 27:2cb1bdb7ae3d 761 return;
dreamselec 2:7a55cb10259f 762 }
dreamselec 15:777390eb5afd 763
dreamselec 27:2cb1bdb7ae3d 764 void printColourSensorInfoOnLCD(int colourValues[])
dreamselec 27:2cb1bdb7ae3d 765 {
dreamselec 27:2cb1bdb7ae3d 766 float weightedValues[4];
dreamselec 27:2cb1bdb7ae3d 767
dreamselec 27:2cb1bdb7ae3d 768 for (int i = 0; i < 3; i++) {
dreamselec 27:2cb1bdb7ae3d 769 weightedValues[i] = (float)colourValues[i] / (float)colourValues[3];
dreamselec 27:2cb1bdb7ae3d 770 }
dreamselec 27:2cb1bdb7ae3d 771 //TODO: Print values on LCD
dreamselec 27:2cb1bdb7ae3d 772 lcd->cls();
dreamselec 27:2cb1bdb7ae3d 773 lcd->locate(0,0);
dreamselec 27:2cb1bdb7ae3d 774 lcd->printf("Colour sensor");
dreamselec 27:2cb1bdb7ae3d 775 lcd->locate(1,0);
dreamselec 27:2cb1bdb7ae3d 776 lcd->printf("Test mode");
dreamselec 25:792540d69c49 777 }
dreamselec 25:792540d69c49 778
dreamselec 27:2cb1bdb7ae3d 779 void printColourSensorInfoOnPC(int colourValues[])
dreamselec 27:2cb1bdb7ae3d 780 {
dreamselec 27:2cb1bdb7ae3d 781 pc.printf(":<colour_sensor>red=%i,green=%i,blue=%i,clear=%i;", colourValues[0], colourValues[1], colourValues[2], colourValues[3]);
dreamselec 25:792540d69c49 782 }
dreamselec 25:792540d69c49 783
dreamselec 27:2cb1bdb7ae3d 784 void runInColourSensorTestMode()
dreamselec 27:2cb1bdb7ae3d 785 {
dreamselec 27:2cb1bdb7ae3d 786 turnOffTopLEDs();
dreamselec 27:2cb1bdb7ae3d 787 i2cport->write_bit(1, 15);
dreamselec 27:2cb1bdb7ae3d 788 i2cport->write_bit(1, 12);
dreamselec 27:2cb1bdb7ae3d 789 lcd->cls();
dreamselec 27:2cb1bdb7ae3d 790 lcd->locate(0,0);
dreamselec 27:2cb1bdb7ae3d 791 lcd->printf("Colour Sensor");
dreamselec 27:2cb1bdb7ae3d 792 lcd->locate(1,0);
dreamselec 27:2cb1bdb7ae3d 793 lcd->printf("Test Mode");
dreamselec 27:2cb1bdb7ae3d 794 pc.printf("!<colour_sensor>i-time=%.3f", gIntegrationTime);
dreamselec 27:2cb1bdb7ae3d 795
dreamselec 27:2cb1bdb7ae3d 796 int button = 0;
dreamselec 27:2cb1bdb7ae3d 797 bool finished = false;
dreamselec 27:2cb1bdb7ae3d 798
dreamselec 27:2cb1bdb7ae3d 799 do {
dreamselec 27:2cb1bdb7ae3d 800 button = readSwitches();
dreamselec 27:2cb1bdb7ae3d 801
dreamselec 27:2cb1bdb7ae3d 802 if (getColourSensorValue == true) {
dreamselec 27:2cb1bdb7ae3d 803 int colourValues[4];
dreamselec 27:2cb1bdb7ae3d 804 rgbSensor.getAllColors(colourValues);
dreamselec 27:2cb1bdb7ae3d 805 printColourSensorInfoOnPC(colourValues);
dreamselec 27:2cb1bdb7ae3d 806 printColourSensorInfoOnLCD(colourValues);
dreamselec 27:2cb1bdb7ae3d 807 getColourSensorValue = false;
dreamselec 27:2cb1bdb7ae3d 808 } else if (getBlockColourValue == true) {
dreamselec 27:2cb1bdb7ae3d 809 lcd->cls();
dreamselec 27:2cb1bdb7ae3d 810 lcd->locate(0,0);
dreamselec 27:2cb1bdb7ae3d 811 lcd->printf("Drop block");
dreamselec 27:2cb1bdb7ae3d 812
dreamselec 27:2cb1bdb7ae3d 813
dreamselec 27:2cb1bdb7ae3d 814 turnOffTopLEDs();
dreamselec 27:2cb1bdb7ae3d 815
dreamselec 27:2cb1bdb7ae3d 816 while(fpga->getBeamValue(Top) == 0) {
dreamselec 27:2cb1bdb7ae3d 817 myLED1 = fpga->getBeamValue(Top);
dreamselec 27:2cb1bdb7ae3d 818 myLED2 = fpga->getBeamValue(Bottom);
dreamselec 27:2cb1bdb7ae3d 819 }
dreamselec 27:2cb1bdb7ae3d 820
dreamselec 27:2cb1bdb7ae3d 821 myLED3 = 1;
dreamselec 27:2cb1bdb7ae3d 822 int colourValues[3][4];
dreamselec 27:2cb1bdb7ae3d 823 for (int i = 0; i < 3; i++) {
dreamselec 27:2cb1bdb7ae3d 824 memset(colourValues[i], 0, sizeof(colourValues));
dreamselec 27:2cb1bdb7ae3d 825 }
dreamselec 27:2cb1bdb7ae3d 826 int valueCount = 0;
dreamselec 27:2cb1bdb7ae3d 827
dreamselec 27:2cb1bdb7ae3d 828 do {
dreamselec 27:2cb1bdb7ae3d 829 rgbSensor.getAllColors(colourValues[valueCount]);
dreamselec 28:7e4d29977d72 830 valueCount++;
dreamselec 27:2cb1bdb7ae3d 831 } while (fpga->getBeamValue(Top) == 1 && valueCount < 3);
dreamselec 27:2cb1bdb7ae3d 832
dreamselec 27:2cb1bdb7ae3d 833 turnOffTopLEDs();
dreamselec 27:2cb1bdb7ae3d 834 myLED4 = 1;
dreamselec 27:2cb1bdb7ae3d 835
dreamselec 27:2cb1bdb7ae3d 836 int averageValues[4] = {0, 0, 0, 0};
dreamselec 27:2cb1bdb7ae3d 837 for (int i = 0; i < 3; i++) {
dreamselec 27:2cb1bdb7ae3d 838 averageValues[0] += colourValues[i][0];
dreamselec 27:2cb1bdb7ae3d 839 averageValues[1] += colourValues[i][1];
dreamselec 27:2cb1bdb7ae3d 840 averageValues[2] += colourValues[i][2];
dreamselec 27:2cb1bdb7ae3d 841 averageValues[3] += colourValues[i][3];
dreamselec 27:2cb1bdb7ae3d 842 }
dreamselec 27:2cb1bdb7ae3d 843 averageValues[0] = averageValues[0] / valueCount;
dreamselec 27:2cb1bdb7ae3d 844 averageValues[1] = averageValues[1] / valueCount;
dreamselec 27:2cb1bdb7ae3d 845 averageValues[2] = averageValues[2] / valueCount;
dreamselec 27:2cb1bdb7ae3d 846 averageValues[3] = averageValues[3] / valueCount;
dreamselec 27:2cb1bdb7ae3d 847 myLED4 = 0;
dreamselec 27:2cb1bdb7ae3d 848
dreamselec 27:2cb1bdb7ae3d 849 printColourSensorInfoOnPC(averageValues);
dreamselec 27:2cb1bdb7ae3d 850 printColourSensorInfoOnLCD(averageValues);
dreamselec 27:2cb1bdb7ae3d 851
dreamselec 27:2cb1bdb7ae3d 852 getBlockColourValue = false;
dreamselec 27:2cb1bdb7ae3d 853 } else if (button == 1) {
dreamselec 27:2cb1bdb7ae3d 854 getColourSensorValue = true;
dreamselec 27:2cb1bdb7ae3d 855 button = 0;
dreamselec 27:2cb1bdb7ae3d 856 }
dreamselec 27:2cb1bdb7ae3d 857
dreamselec 27:2cb1bdb7ae3d 858 finished = button == 4;
dreamselec 25:792540d69c49 859 // button = 0;
dreamselec 27:2cb1bdb7ae3d 860 } while (finished == false && runColourSensorTest == true);
dreamselec 27:2cb1bdb7ae3d 861
dreamselec 27:2cb1bdb7ae3d 862 turnOffBottomLEDs();
dreamselec 27:2cb1bdb7ae3d 863 lcd->cls();
dreamselec 27:2cb1bdb7ae3d 864 lcd->locate(0,0);
dreamselec 27:2cb1bdb7ae3d 865 lcd->printf("Finished test");
dreamselec 27:2cb1bdb7ae3d 866 if (runColourSensorTest == true) {
dreamselec 27:2cb1bdb7ae3d 867 pc.printf(":<colour_sensor>test=pause;");
dreamselec 27:2cb1bdb7ae3d 868 runColourSensorTest = false;
dreamselec 27:2cb1bdb7ae3d 869 }
dreamselec 27:2cb1bdb7ae3d 870 wait(0.5);
dreamselec 27:2cb1bdb7ae3d 871 return;
dreamselec 25:792540d69c49 872 }
dreamselec 25:792540d69c49 873
dreamselec 15:777390eb5afd 874 void displayWaitingLine()
dreamselec 15:777390eb5afd 875 {
dreamselec 27:2cb1bdb7ae3d 876 lcd->cls();
dreamselec 27:2cb1bdb7ae3d 877 lcd->locate(0,0);
dreamselec 27:2cb1bdb7ae3d 878 lcd->printf("Waiting...");
dreamselec 27:2cb1bdb7ae3d 879 lcd->locate(1,0);
dreamselec 15:777390eb5afd 880 }
dreamselec 15:777390eb5afd 881
dreamselec 15:777390eb5afd 882 void displayPCStatus()
dreamselec 15:777390eb5afd 883 {
dreamselec 27:2cb1bdb7ae3d 884 lcd->cls();
dreamselec 27:2cb1bdb7ae3d 885 lcd->locate(0,0);
dreamselec 27:2cb1bdb7ae3d 886 if (connectedToPC == true) {
dreamselec 27:2cb1bdb7ae3d 887 if (currentMode == Normal)
dreamselec 27:2cb1bdb7ae3d 888 lcd->printf("Normal mode.");
dreamselec 27:2cb1bdb7ae3d 889 else if (currentMode == Maintanence)
dreamselec 27:2cb1bdb7ae3d 890 lcd->printf("Maintanence.");
dreamselec 27:2cb1bdb7ae3d 891 else if (currentMode == None)
dreamselec 27:2cb1bdb7ae3d 892 lcd->printf("Connected to PC");
dreamselec 27:2cb1bdb7ae3d 893 } else
dreamselec 27:2cb1bdb7ae3d 894 lcd->printf("Waiting for PC..");
dreamselec 15:777390eb5afd 895
dreamselec 27:2cb1bdb7ae3d 896 lcd->locate(1,0);
dreamselec 27:2cb1bdb7ae3d 897 lcd->printf("4:Disconnect");
dreamselec 27:2cb1bdb7ae3d 898 D_LEDS_OFF();
dreamselec 27:2cb1bdb7ae3d 899 i2cport->write_bit(1,15);
dreamselec 18:44a1c1a30166 900 }
dreamselec 23:db91aaa43a9e 901
dreamselec 27:2cb1bdb7ae3d 902 void turnOffTopLEDs()
dreamselec 27:2cb1bdb7ae3d 903 {
dreamselec 27:2cb1bdb7ae3d 904 myLED1 = 0;
dreamselec 27:2cb1bdb7ae3d 905 myLED2 = 0;
dreamselec 27:2cb1bdb7ae3d 906 myLED3 = 0;
dreamselec 27:2cb1bdb7ae3d 907 myLED4 = 0;
dreamselec 23:db91aaa43a9e 908 }
dreamselec 23:db91aaa43a9e 909
dreamselec 27:2cb1bdb7ae3d 910 void turnOffBottomLEDs()
dreamselec 27:2cb1bdb7ae3d 911 {
dreamselec 27:2cb1bdb7ae3d 912 i2cport->write_bit(0, 12);
dreamselec 27:2cb1bdb7ae3d 913 i2cport->write_bit(0, 13);
dreamselec 27:2cb1bdb7ae3d 914 i2cport->write_bit(0, 14);
dreamselec 27:2cb1bdb7ae3d 915 i2cport->write_bit(0, 15);
dreamselec 23:db91aaa43a9e 916 }