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@32:9a4046224b11, 2015-12-17 (annotated)

Committer:
dreamselec
Date:
Thu Dec 17 17:23:24 2015 +0100
Revision:
32:9a4046224b11
Parent:
31:16e056b9f9e0 
Comments and clean up.

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