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@20:4e0f0944f28f, 2015-11-24 (annotated)

Committer:
dreamselec
Date:
Tue Nov 24 20:53:51 2015 +0000
Revision:
20:4e0f0944f28f
Parent:
19:61b21ac4896e
Child:
21:60c4fe94d79c
Hardcoded red small block values. Testing with hardware was working.

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 2:7a55cb10259f 31
dreamselec 6:98fe30430194 32 Commander _commander = Commander();
dreamselec 6:98fe30430194 33 Commander *commander = &_commander;
dreamselec 6:98fe30430194 34 FPGA _fpga = FPGA();
dreamselec 9:dc8f155b71c8 35 extern FPGA *fpga = &_fpga;
dreamselec 6:98fe30430194 36
dreamselec 20:4e0f0944f28f 37 Block defualtHazBlock = Block();
dreamselec 6:98fe30430194 38 extern Block _HazBlock;
dreamselec 6:98fe30430194 39 Block *HazBlock = &_HazBlock;
dreamselec 6:98fe30430194 40
dreamselec 18:44a1c1a30166 41 extern PCModes currentMode;
dreamselec 18:44a1c1a30166 42
dreamselec 6:98fe30430194 43 void initInternal();
dreamselec 7:b6e31bfdb2af 44 void initPort(int baudRate=kDefaultBaudRate);
dreamselec 6:98fe30430194 45 void printPCDetectedText();
dreamselec 20:4e0f0944f28f 46 //bool displayAbortDialog();
dreamselec 6:98fe30430194 47 void Rx_interrupt();
dreamselec 18:44a1c1a30166 48 bool checkColour(int colourValues[]);
dreamselec 10:16ba52f8e025 49 void runInServoTestMode();
dreamselec 15:777390eb5afd 50 void displayWaitingLine();
dreamselec 15:777390eb5afd 51 void displayPCStatus();
dreamselec 18:44a1c1a30166 52 bool waitForBlock();
dreamselec 18:44a1c1a30166 53 void sortBlock();
dreamselec 6:98fe30430194 54
dreamselec 6:98fe30430194 55 int main()
dreamselec 6:98fe30430194 56 {
dreamselec 19:61b21ac4896e 57 initInternal();
dreamselec 19:61b21ac4896e 58 initPort();
dreamselec 19:61b21ac4896e 59 srand((unsigned)time(NULL));
dreamselec 19:61b21ac4896e 60 U_LEDS_OFF();
dreamselec 19:61b21ac4896e 61 lcd->cls();
dreamselec 15:777390eb5afd 62
dreamselec 19:61b21ac4896e 63 rgbSensor.enablePowerAndRGBC();
dreamselec 20:4e0f0944f28f 64 rgbSensor.setIntegrationTime(3);
dreamselec 6:98fe30430194 65
dreamselec 19:61b21ac4896e 66 // Create a serial intereput for RxIrq so when PC is connected it sends '$' to tell MBED it's there.
dreamselec 19:61b21ac4896e 67 // https://developer.mbed.org/cookbook/Serial-Interrupts
dreamselec 19:61b21ac4896e 68 pc.attach(&Rx_interrupt, Serial::RxIrq);
dreamselec 6:98fe30430194 69
dreamselec 19:61b21ac4896e 70 for (;;) {
dreamselec 19:61b21ac4896e 71 lcd->cls();
dreamselec 19:61b21ac4896e 72 i2cport->write_bit(1, 12);
dreamselec 19:61b21ac4896e 73 lcd->printf("1: Start sorting.");
dreamselec 19:61b21ac4896e 74 LCDSL();
dreamselec 19:61b21ac4896e 75 i2cport->write_bit(1,13);
dreamselec 19:61b21ac4896e 76 lcd->printf("2: Connect to PC");
dreamselec 15:777390eb5afd 77
dreamselec 19:61b21ac4896e 78 fpga->moveStoppingServo(Stop);
dreamselec 19:61b21ac4896e 79 fpga->moveSortingServo(NonHaz);
dreamselec 10:16ba52f8e025 80
dreamselec 19:61b21ac4896e 81 int selection = 0;
dreamselec 19:61b21ac4896e 82 do {
dreamselec 19:61b21ac4896e 83 myLED4 = selection;
dreamselec 19:61b21ac4896e 84 selection = readSwitches();
dreamselec 19:61b21ac4896e 85 } while (selection != 1 && selection != 2 && connectedToPC == false);
dreamselec 19:61b21ac4896e 86 D_LEDS_OFF();
dreamselec 19:61b21ac4896e 87 if (selection == 1) {
dreamselec 19:61b21ac4896e 88 // User selected op 1: Start sorting autonomously.
dreamselec 19:61b21ac4896e 89 i2cport->write_bit(1, 12);
dreamselec 19:61b21ac4896e 90 lcd->cls();
dreamselec 19:61b21ac4896e 91 LCDFL();
dreamselec 19:61b21ac4896e 92 lcd->printf("Starting sorting");
dreamselec 19:61b21ac4896e 93 wait(0.5);
dreamselec 19:61b21ac4896e 94 D_LEDS_OFF();
dreamselec 19:61b21ac4896e 95
dreamselec 19:61b21ac4896e 96 for(;;) {
dreamselec 19:61b21ac4896e 97 displayWaitingLine();
dreamselec 19:61b21ac4896e 98 lcd->printf("for block.");
dreamselec 10:16ba52f8e025 99
dreamselec 19:61b21ac4896e 100 i2cport->write_bit(1, 15);
dreamselec 10:16ba52f8e025 101
dreamselec 19:61b21ac4896e 102 // Break and return to main menu i.e. Start Op, or Connect to PC.
dreamselec 19:61b21ac4896e 103 if (waitForBlock() == false) {
dreamselec 19:61b21ac4896e 104 D_LEDS_OFF();
dreamselec 19:61b21ac4896e 105 break;
dreamselec 19:61b21ac4896e 106 } else {
dreamselec 19:61b21ac4896e 107 sortBlock();
dreamselec 19:61b21ac4896e 108 }
dreamselec 19:61b21ac4896e 109 }
dreamselec 19:61b21ac4896e 110 }
dreamselec 10:16ba52f8e025 111
dreamselec 19:61b21ac4896e 112 if (selection == 2 || connectedToPC == true) {
dreamselec 19:61b21ac4896e 113 for (;;) {
dreamselec 19:61b21ac4896e 114 displayPCStatus();
dreamselec 19:61b21ac4896e 115
dreamselec 19:61b21ac4896e 116 i2cport->write_bit(1, 15);
dreamselec 19:61b21ac4896e 117 int abortOperation = false;
dreamselec 19:61b21ac4896e 118 while (connectedToPC == false && abortOperation == false) {
dreamselec 19:61b21ac4896e 119 abortOperation = readSwitches() == 4;
dreamselec 19:61b21ac4896e 120 }
dreamselec 18:44a1c1a30166 121
dreamselec 19:61b21ac4896e 122 if (abortOperation == true) {
dreamselec 19:61b21ac4896e 123 D_LEDS_OFF();
dreamselec 19:61b21ac4896e 124 break;
dreamselec 19:61b21ac4896e 125 }
dreamselec 18:44a1c1a30166 126
dreamselec 19:61b21ac4896e 127 displayPCStatus();
dreamselec 15:777390eb5afd 128
dreamselec 19:61b21ac4896e 129 while (abortOperation == false && connectedToPC == true) {
dreamselec 19:61b21ac4896e 130 if (currentMode == Maintanence) {
dreamselec 19:61b21ac4896e 131 if (runServoTest == true)
dreamselec 19:61b21ac4896e 132 runInServoTestMode();
dreamselec 19:61b21ac4896e 133 } else if (currentMode == Normal) {
dreamselec 19:61b21ac4896e 134 displayPCStatus();
dreamselec 19:61b21ac4896e 135 while (currentMode == Normal) {
dreamselec 19:61b21ac4896e 136 if (currentState == Pause) {
dreamselec 19:61b21ac4896e 137 lcd->cls();
dreamselec 19:61b21ac4896e 138 lcd->locate(0,0);
dreamselec 19:61b21ac4896e 139 lcd->printf("Sorting Paused.");
dreamselec 19:61b21ac4896e 140 lcd->locate(1,0);
dreamselec 19:61b21ac4896e 141 lcd->printf("1: Start");
dreamselec 19:61b21ac4896e 142 int button = 0;
dreamselec 20:4e0f0944f28f 143 i2cport->write_bit(1, 12);
dreamselec 20:4e0f0944f28f 144 i2cport->write_bit(1, 15);
dreamselec 20:4e0f0944f28f 145 while (currentState == Pause && currentMode == Normal) {
dreamselec 19:61b21ac4896e 146 button = readSwitches();
dreamselec 19:61b21ac4896e 147 if (button == 1) {
dreamselec 19:61b21ac4896e 148 pc.printf("state:start\n");
dreamselec 19:61b21ac4896e 149 currentState = Start;
dreamselec 19:61b21ac4896e 150 } else if (button == 4) {
dreamselec 20:4e0f0944f28f 151 currentMode = None;
dreamselec 20:4e0f0944f28f 152 // goto setModeNone;
dreamselec 20:4e0f0944f28f 153 break;
dreamselec 19:61b21ac4896e 154 }
dreamselec 19:61b21ac4896e 155 }
dreamselec 19:61b21ac4896e 156 }
dreamselec 19:61b21ac4896e 157 if (currentState == Start) {
dreamselec 20:4e0f0944f28f 158 lcd->cls();
dreamselec 20:4e0f0944f28f 159 lcd->locate(0,0);
dreamselec 20:4e0f0944f28f 160 lcd->printf("Sorting mode...");
dreamselec 20:4e0f0944f28f 161 while (currentState == Start && currentMode == Normal) {
dreamselec 19:61b21ac4896e 162 if (waitForBlock() == false) {
dreamselec 19:61b21ac4896e 163 if (connectedToPC == true) {
dreamselec 19:61b21ac4896e 164 // TODO: Tell PC to update UI if aborted from MBED.
dreamselec 19:61b21ac4896e 165 pc.printf("state:pause\n");
dreamselec 20:4e0f0944f28f 166 currentState = Pause;
dreamselec 19:61b21ac4896e 167 continue;
dreamselec 19:61b21ac4896e 168 } else if (connectedToPC == false) {
dreamselec 19:61b21ac4896e 169 currentMode = None;
dreamselec 19:61b21ac4896e 170 abortOperation = true;
dreamselec 19:61b21ac4896e 171 }
dreamselec 19:61b21ac4896e 172 } else {
dreamselec 19:61b21ac4896e 173 sortBlock();
dreamselec 19:61b21ac4896e 174 }
dreamselec 19:61b21ac4896e 175 }
dreamselec 19:61b21ac4896e 176 }
dreamselec 19:61b21ac4896e 177 }
dreamselec 19:61b21ac4896e 178 } else if (currentMode == None) {
dreamselec 19:61b21ac4896e 179 setModeNone:
dreamselec 20:4e0f0944f28f 180 D_LEDS_OFF();
dreamselec 20:4e0f0944f28f 181 i2cport->write_bit(1,15);
dreamselec 19:61b21ac4896e 182 displayPCStatus();
dreamselec 20:4e0f0944f28f 183 while (currentMode == None && abortOperation == false && connectedToPC == true) {
dreamselec 20:4e0f0944f28f 184 if (i2cport->read_bit(11)) {
dreamselec 20:4e0f0944f28f 185 abortOperation = displayAbortDialog();
dreamselec 20:4e0f0944f28f 186 // Cancel the Abort
dreamselec 20:4e0f0944f28f 187 if (abortOperation == false) {
dreamselec 20:4e0f0944f28f 188 displayPCStatus();
dreamselec 20:4e0f0944f28f 189 i2cport->write_bit(1, 15);
dreamselec 20:4e0f0944f28f 190 }
dreamselec 20:4e0f0944f28f 191 }
dreamselec 19:61b21ac4896e 192 }
dreamselec 19:61b21ac4896e 193 }
dreamselec 19:61b21ac4896e 194 }
dreamselec 15:777390eb5afd 195
dreamselec 19:61b21ac4896e 196 if (abortOperation == true ) {
dreamselec 19:61b21ac4896e 197 connectedToPC = false;
dreamselec 19:61b21ac4896e 198 D_LEDS_OFF();
dreamselec 19:61b21ac4896e 199 break;
dreamselec 19:61b21ac4896e 200 }
dreamselec 19:61b21ac4896e 201 }
dreamselec 19:61b21ac4896e 202 }
dreamselec 19:61b21ac4896e 203
dreamselec 19:61b21ac4896e 204 }
dreamselec 18:44a1c1a30166 205 }
dreamselec 8:e1da2ae62885 206
dreamselec 18:44a1c1a30166 207 // Waits until detects block.
dreamselec 18:44a1c1a30166 208 // true if block detected, false if cancelled/connected to PC.
dreamselec 19:61b21ac4896e 209 bool waitForBlock()
dreamselec 19:61b21ac4896e 210 {
dreamselec 20:4e0f0944f28f 211 myLED4 = 0;
dreamselec 20:4e0f0944f28f 212 myLED1 = 1;
dreamselec 19:61b21ac4896e 213 if (connectedToPC == false) {
dreamselec 19:61b21ac4896e 214 bool abortOperation = false;
dreamselec 19:61b21ac4896e 215 int blockInserted = 0;
dreamselec 19:61b21ac4896e 216 // Wait until a block is breaking the beam, or button 4 is pressed to abort.
dreamselec 19:61b21ac4896e 217 do {
dreamselec 19:61b21ac4896e 218 blockInserted = fpga->getBeamValue(Top);
dreamselec 19:61b21ac4896e 219 myLED4 = blockInserted;
dreamselec 19:61b21ac4896e 220 if (i2cport->read_bit(11)) {
dreamselec 19:61b21ac4896e 221 abortOperation = displayAbortDialog();
dreamselec 19:61b21ac4896e 222 // Cancel the Abort
dreamselec 19:61b21ac4896e 223 if (abortOperation == false) {
dreamselec 19:61b21ac4896e 224 displayWaitingLine();
dreamselec 19:61b21ac4896e 225 lcd->printf("for block");
dreamselec 19:61b21ac4896e 226 i2cport->write_bit(1, 15);
dreamselec 19:61b21ac4896e 227 }
dreamselec 19:61b21ac4896e 228 }
dreamselec 19:61b21ac4896e 229 } while (abortOperation == false && blockInserted != 1 && connectedToPC == false);
dreamselec 18:44a1c1a30166 230
dreamselec 19:61b21ac4896e 231 if (abortOperation == true || connectedToPC == true)
dreamselec 19:61b21ac4896e 232 return false;
dreamselec 19:61b21ac4896e 233 else
dreamselec 19:61b21ac4896e 234 return true;
dreamselec 19:61b21ac4896e 235 } else if (connectedToPC == true) {
dreamselec 19:61b21ac4896e 236 bool abortOperation = false;
dreamselec 19:61b21ac4896e 237 int blockInserted = 0;
dreamselec 19:61b21ac4896e 238 // Wait until a block is breaking the beam, or button 4 is pressed to abort.
dreamselec 19:61b21ac4896e 239 do {
dreamselec 19:61b21ac4896e 240 blockInserted = fpga->getBeamValue(Top);
dreamselec 19:61b21ac4896e 241 myLED4 = blockInserted;
dreamselec 19:61b21ac4896e 242 if (i2cport->read_bit(11)) {
dreamselec 19:61b21ac4896e 243 abortOperation = displayAbortDialog();
dreamselec 19:61b21ac4896e 244 // Cancel the Abort
dreamselec 19:61b21ac4896e 245 if (abortOperation == false) {
dreamselec 19:61b21ac4896e 246 displayWaitingLine();
dreamselec 19:61b21ac4896e 247 lcd->printf("for block");
dreamselec 19:61b21ac4896e 248 i2cport->write_bit(1, 15);
dreamselec 19:61b21ac4896e 249 }
dreamselec 19:61b21ac4896e 250 }
dreamselec 20:4e0f0944f28f 251 } while (abortOperation == false && blockInserted != 1 && connectedToPC == true && currentState == Start && currentMode == Normal);
dreamselec 19:61b21ac4896e 252
dreamselec 20:4e0f0944f28f 253 if (abortOperation == true || connectedToPC == false || currentState == Pause || currentMode == Maintanence)
dreamselec 19:61b21ac4896e 254 return false;
dreamselec 19:61b21ac4896e 255 else
dreamselec 19:61b21ac4896e 256 return true;
dreamselec 19:61b21ac4896e 257 }
dreamselec 18:44a1c1a30166 258 }
dreamselec 14:cf2f255b5560 259
dreamselec 19:61b21ac4896e 260 void sortBlock()
dreamselec 19:61b21ac4896e 261 {
dreamselec 20:4e0f0944f28f 262 myLED1 = 0;
dreamselec 20:4e0f0944f28f 263 myLED2 = 1;
dreamselec 19:61b21ac4896e 264 // Cannot Abort any longer. Block is inserted.
dreamselec 19:61b21ac4896e 265 // Detach rx interrupt until block processed.
dreamselec 19:61b21ac4896e 266 NVIC_DisableIRQ(UART1_IRQn);
dreamselec 19:61b21ac4896e 267 int colourValues[4];
dreamselec 19:61b21ac4896e 268 rgbSensor.getAllColors(colourValues);
dreamselec 18:44a1c1a30166 269
dreamselec 19:61b21ac4896e 270 int canCheckForSize = fpga->checkForBlock();
dreamselec 19:61b21ac4896e 271 int blockSize = 0;
dreamselec 18:44a1c1a30166 272
dreamselec 19:61b21ac4896e 273 while (canCheckForSize == 0) {
dreamselec 19:61b21ac4896e 274 canCheckForSize = fpga->checkForBlock();
dreamselec 19:61b21ac4896e 275 }
dreamselec 15:777390eb5afd 276
dreamselec 19:61b21ac4896e 277 blockSize = fpga->checkForSize();
dreamselec 19:61b21ac4896e 278 bool haz = false;
dreamselec 6:98fe30430194 279
dreamselec 19:61b21ac4896e 280 if (blockSize == HazBlock->size) {
dreamselec 19:61b21ac4896e 281 haz = checkColour(colourValues);
dreamselec 19:61b21ac4896e 282 } else {
dreamselec 19:61b21ac4896e 283 fpga->moveStoppingServo(Go);
dreamselec 19:61b21ac4896e 284 while(fpga->checkForSize()) {}
dreamselec 19:61b21ac4896e 285 fpga->moveStoppingServo(Stop);
dreamselec 19:61b21ac4896e 286 }
dreamselec 10:16ba52f8e025 287
dreamselec 20:4e0f0944f28f 288
dreamselec 19:61b21ac4896e 289 if (connectedToPC)
dreamselec 19:61b21ac4896e 290 pc.printf("BLOCK:Size:%i,Red:%i,Green:%i,Blue:%i,Clear:%i,Haz:%i;", blockSize, colourValues[0], colourValues[1], colourValues[2], colourValues[3], haz);
dreamselec 18:44a1c1a30166 291
dreamselec 19:61b21ac4896e 292 // Re-Attach rx interrupt
dreamselec 19:61b21ac4896e 293 NVIC_EnableIRQ(UART1_IRQn);
dreamselec 20:4e0f0944f28f 294 myLED3 = 0;
dreamselec 20:4e0f0944f28f 295 myLED4 = 1;
dreamselec 19:61b21ac4896e 296 return;
dreamselec 6:98fe30430194 297 }
dreamselec 6:98fe30430194 298
dreamselec 6:98fe30430194 299 /// Called every-time it receives an char from PC.
dreamselec 10:16ba52f8e025 300 void Rx_interrupt()
dreamselec 10:16ba52f8e025 301 {
dreamselec 19:61b21ac4896e 302 char interruptChar = pc.getc();
dreamselec 19:61b21ac4896e 303 // Uncomment to Echo to USB serial to watch data flow
dreamselec 19:61b21ac4896e 304 // pc.putc(interruptChar);
dreamselec 6:98fe30430194 305
dreamselec 19:61b21ac4896e 306 NVIC_DisableIRQ(UART1_IRQn);
dreamselec 8:e1da2ae62885 307
dreamselec 19:61b21ac4896e 308 if (interruptChar == CommandTypeValue[Query]) {
dreamselec 19:61b21ac4896e 309 commander->decodeCommand(Query);
dreamselec 19:61b21ac4896e 310 } else if (interruptChar == CommandTypeValue[Set]) {
dreamselec 19:61b21ac4896e 311 commander->decodeCommand(Set);
dreamselec 19:61b21ac4896e 312 } else if (interruptChar== CommandTypeValue[Reply]) {
dreamselec 19:61b21ac4896e 313 commander->decodeCommand(Reply);
dreamselec 19:61b21ac4896e 314 }
dreamselec 8:e1da2ae62885 315
dreamselec 19:61b21ac4896e 316 NVIC_EnableIRQ(UART1_IRQn);
dreamselec 6:98fe30430194 317 }
dreamselec 3:843b830ee8bd 318
dreamselec 4:4eebb4de22a7 319 void initInternal()
dreamselec 4:4eebb4de22a7 320 {
dreamselec 20:4e0f0944f28f 321 myLED1 = 1;
dreamselec 19:61b21ac4896e 322 i2cport = new MCP23017(p9, p10, 0x40);
dreamselec 19:61b21ac4896e 323 lcd = new WattBob_TextLCD(i2cport);
dreamselec 19:61b21ac4896e 324 BACKLIGHT_ON(i2cport);
dreamselec 19:61b21ac4896e 325 lcd->cls();
dreamselec 20:4e0f0944f28f 326 lcd->locate(0,0);
dreamselec 19:61b21ac4896e 327 lcd->printf("Initilizing...");
dreamselec 20:4e0f0944f28f 328 DefaultHazBlock();
dreamselec 19:61b21ac4896e 329 myLED2 = 1;
dreamselec 19:61b21ac4896e 330 return;
dreamselec 3:843b830ee8bd 331 }
dreamselec 3:843b830ee8bd 332
dreamselec 7:b6e31bfdb2af 333 void initPort(int baudRate)
dreamselec 4:4eebb4de22a7 334 {
dreamselec 19:61b21ac4896e 335 myLED3 = 1;
dreamselec 19:61b21ac4896e 336 pc.baud(baudRate);
dreamselec 19:61b21ac4896e 337 pc.format(8, SerialBase::None, gStopBits);
dreamselec 19:61b21ac4896e 338 myLED4 = 1;
dreamselec 19:61b21ac4896e 339 wait (0.1);
dreamselec 19:61b21ac4896e 340 return;
dreamselec 3:843b830ee8bd 341 }
dreamselec 3:843b830ee8bd 342
dreamselec 18:44a1c1a30166 343 bool checkColour(int colourValues[])
dreamselec 15:777390eb5afd 344 {
dreamselec 20:4e0f0944f28f 345 myLED2 = 0;
dreamselec 20:4e0f0944f28f 346 myLED3 = 1;
dreamselec 20:4e0f0944f28f 347 bool isHazColour[3] = {false, false, false };
dreamselec 20:4e0f0944f28f 348
dreamselec 20:4e0f0944f28f 349 float adjustedValues[3];
dreamselec 20:4e0f0944f28f 350 float percentageError[3];
dreamselec 20:4e0f0944f28f 351
dreamselec 15:777390eb5afd 352
dreamselec 20:4e0f0944f28f 353 for (int i = 0; i < 3; i++) {
dreamselec 20:4e0f0944f28f 354 adjustedValues[i] = (float)colourValues[i]/(float)colourValues[3];
dreamselec 20:4e0f0944f28f 355 percentageError[i] = (adjustedValues[i] - kAverageRedBlock[i]) / kAverageRedBlock[i];
dreamselec 20:4e0f0944f28f 356 pc.printf("DEBUG:Percentage Error: %.5f.\n", percentageError[i]);
dreamselec 20:4e0f0944f28f 357
dreamselec 20:4e0f0944f28f 358 if ((percentageError[i] < 0 && std::abs(percentageError[i]) < kMinError[i] * 2) || percentageError[i] == 0) {
dreamselec 20:4e0f0944f28f 359 pc.printf("DEBUG:%i.\n", i);
dreamselec 19:61b21ac4896e 360 isHazColour[i] = true;
dreamselec 20:4e0f0944f28f 361 } else if (percentageError[i] > 0 && percentageError[i] < kMaxError[i] * 2) {
dreamselec 20:4e0f0944f28f 362 isHazColour[i] = true;
dreamselec 20:4e0f0944f28f 363 pc.printf("DEBUG:%i.\n", i);
dreamselec 19:61b21ac4896e 364 }
dreamselec 19:61b21ac4896e 365 }
dreamselec 15:777390eb5afd 366
dreamselec 20:4e0f0944f28f 367 // for (int i = 0; i < 3; i++) {
dreamselec 20:4e0f0944f28f 368 // if (colourValues[i] < HazBlock->maxColour.components[i] && colourValues[i] > HazBlock->minColour.components[i]) {
dreamselec 20:4e0f0944f28f 369 // isHazColour[i] = true;
dreamselec 20:4e0f0944f28f 370 // }
dreamselec 20:4e0f0944f28f 371 // }
dreamselec 15:777390eb5afd 372
dreamselec 19:61b21ac4896e 373 bool isHazBlock = false;
dreamselec 15:777390eb5afd 374
dreamselec 20:4e0f0944f28f 375 if (isHazColour[0] && isHazColour[1] && isHazColour[2]) {
dreamselec 19:61b21ac4896e 376 fpga->moveSortingServo(Haz);
dreamselec 19:61b21ac4896e 377 fpga->moveStoppingServo(Go);
dreamselec 19:61b21ac4896e 378 while(fpga->checkForBlock()) {}
dreamselec 20:4e0f0944f28f 379 wait(kServoWait);
dreamselec 19:61b21ac4896e 380 fpga->moveStoppingServo(Stop);
dreamselec 19:61b21ac4896e 381 fpga->moveSortingServo(NonHaz);
dreamselec 20:4e0f0944f28f 382 isHazBlock = true;
dreamselec 19:61b21ac4896e 383 } else {
dreamselec 19:61b21ac4896e 384 fpga->moveStoppingServo(Go);
dreamselec 19:61b21ac4896e 385 while(fpga->checkForBlock()) {}
dreamselec 19:61b21ac4896e 386 fpga->moveStoppingServo(Stop);
dreamselec 20:4e0f0944f28f 387 isHazBlock = false;
dreamselec 19:61b21ac4896e 388 }
dreamselec 20:4e0f0944f28f 389
dreamselec 20:4e0f0944f28f 390 pc.printf("BLOCK:Red:%.3f,Green:%.3f,Blue:%.3f,Haz:%i;", adjustedValues[0], adjustedValues[1], adjustedValues[2], isHazBlock);
dreamselec 20:4e0f0944f28f 391 return isHazBlock;
dreamselec 3:843b830ee8bd 392 }
dreamselec 3:843b830ee8bd 393
dreamselec 6:98fe30430194 394 void printPCDetectedText()
dreamselec 4:4eebb4de22a7 395 {
dreamselec 19:61b21ac4896e 396 lcd->cls();
dreamselec 19:61b21ac4896e 397 LCDFL();
dreamselec 19:61b21ac4896e 398 lcd->printf("Detected PC.");
dreamselec 19:61b21ac4896e 399 LCDSL();
dreamselec 19:61b21ac4896e 400 lcd->printf("Connecting");
dreamselec 19:61b21ac4896e 401 initPort();
dreamselec 10:16ba52f8e025 402 }
dreamselec 10:16ba52f8e025 403
dreamselec 10:16ba52f8e025 404 bool displayAbortDialog()
dreamselec 10:16ba52f8e025 405 {
dreamselec 19:61b21ac4896e 406 while (i2cport->read_bit(11) == 1) {}
dreamselec 19:61b21ac4896e 407 i2cport->write_bit(1, 12);
dreamselec 10:16ba52f8e025 408
dreamselec 19:61b21ac4896e 409 lcd->cls();
dreamselec 19:61b21ac4896e 410 LCDFL();
dreamselec 19:61b21ac4896e 411 lcd->printf("Abort?");
dreamselec 19:61b21ac4896e 412 LCDSL();
dreamselec 19:61b21ac4896e 413 lcd->printf("1:Yes, 2,3,4:No");
dreamselec 19:61b21ac4896e 414 int reply = 0;
dreamselec 19:61b21ac4896e 415 do {
dreamselec 19:61b21ac4896e 416 reply = readSwitches();
dreamselec 19:61b21ac4896e 417 } while(reply == 0);
dreamselec 10:16ba52f8e025 418
dreamselec 19:61b21ac4896e 419 D_LEDS_OFF();
dreamselec 19:61b21ac4896e 420 if (reply == 1) {
dreamselec 19:61b21ac4896e 421 // while (i2cport->read_bit(8)) { }
dreamselec 19:61b21ac4896e 422 return true;
dreamselec 19:61b21ac4896e 423 } else {
dreamselec 19:61b21ac4896e 424 // while (i2cport->read_bit(9) || i2cport->read_bit(10) || i2cport->read_bit(11)) { }
dreamselec 19:61b21ac4896e 425 return false;
dreamselec 19:61b21ac4896e 426 }
dreamselec 3:843b830ee8bd 427 }
dreamselec 3:843b830ee8bd 428
dreamselec 15:777390eb5afd 429 void printServoInfoOnLCD()
dreamselec 15:777390eb5afd 430 {
dreamselec 19:61b21ac4896e 431 lcd->cls();
dreamselec 19:61b21ac4896e 432 lcd->locate(0,0);
dreamselec 19:61b21ac4896e 433 lcd->printf("1: Stop servo:%i", fpga->stoppingServoPosition);
dreamselec 19:61b21ac4896e 434 lcd->locate(1,0);
dreamselec 19:61b21ac4896e 435 lcd->printf("2: Sort servo:%i", fpga->sortingServoPosition);
dreamselec 10:16ba52f8e025 436 }
dreamselec 10:16ba52f8e025 437
dreamselec 15:777390eb5afd 438 void runInServoTestMode()
dreamselec 15:777390eb5afd 439 {
dreamselec 19:61b21ac4896e 440 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 19:61b21ac4896e 441 printServoInfoOnLCD();
dreamselec 19:61b21ac4896e 442 int finished = false;
dreamselec 15:777390eb5afd 443
dreamselec 19:61b21ac4896e 444 i2cport->write_bit(1, 12);
dreamselec 19:61b21ac4896e 445 i2cport->write_bit(1, 13);
dreamselec 19:61b21ac4896e 446 i2cport->write_bit(1, 15);
dreamselec 19:61b21ac4896e 447 int button = 0;
dreamselec 19:61b21ac4896e 448 do {
dreamselec 19:61b21ac4896e 449 button = readSwitches();
dreamselec 19:61b21ac4896e 450 if (button == 1) {
dreamselec 19:61b21ac4896e 451 fpga->toggleStoppingServo();
dreamselec 19:61b21ac4896e 452 printServoInfoOnLCD();
dreamselec 20:4e0f0944f28f 453 wait(kServoWait);
dreamselec 19:61b21ac4896e 454 } else if (button == 2) {
dreamselec 19:61b21ac4896e 455 fpga->toggleSortingServo();
dreamselec 19:61b21ac4896e 456 printServoInfoOnLCD();
dreamselec 20:4e0f0944f28f 457 wait(kServoWait);
dreamselec 19:61b21ac4896e 458 }
dreamselec 19:61b21ac4896e 459 finished = button == 4;
dreamselec 19:61b21ac4896e 460 button = 0;
dreamselec 19:61b21ac4896e 461 } while (finished == false && runServoTest != false);
dreamselec 18:44a1c1a30166 462
dreamselec 19:61b21ac4896e 463 D_LEDS_OFF();
dreamselec 19:61b21ac4896e 464 lcd->cls();
dreamselec 19:61b21ac4896e 465 lcd->locate(0,0);
dreamselec 19:61b21ac4896e 466 lcd->printf("Done servo test");
dreamselec 19:61b21ac4896e 467 //TODO: Inform PC of end.
dreamselec 19:61b21ac4896e 468 wait(0.5);
dreamselec 19:61b21ac4896e 469 lcd->cls();
dreamselec 19:61b21ac4896e 470 return;
dreamselec 2:7a55cb10259f 471 }
dreamselec 15:777390eb5afd 472
dreamselec 15:777390eb5afd 473 void displayWaitingLine()
dreamselec 15:777390eb5afd 474 {
dreamselec 19:61b21ac4896e 475 lcd->cls();
dreamselec 19:61b21ac4896e 476 lcd->locate(0,0);
dreamselec 19:61b21ac4896e 477 lcd->printf("Waiting...");
dreamselec 19:61b21ac4896e 478 lcd->locate(1,0);
dreamselec 15:777390eb5afd 479 }
dreamselec 15:777390eb5afd 480
dreamselec 15:777390eb5afd 481 void displayPCStatus()
dreamselec 15:777390eb5afd 482 {
dreamselec 19:61b21ac4896e 483 lcd->cls();
dreamselec 19:61b21ac4896e 484 lcd->locate(0,0);
dreamselec 19:61b21ac4896e 485 if (connectedToPC) {
dreamselec 19:61b21ac4896e 486 if (currentMode == Normal)
dreamselec 19:61b21ac4896e 487 lcd->printf("Normal mode.");
dreamselec 19:61b21ac4896e 488 else if (currentMode == Maintanence)
dreamselec 19:61b21ac4896e 489 lcd->printf("Maintanence.");
dreamselec 19:61b21ac4896e 490 else if (currentMode == None)
dreamselec 19:61b21ac4896e 491 lcd->printf("Connected to PC");
dreamselec 19:61b21ac4896e 492 } else
dreamselec 19:61b21ac4896e 493 lcd->printf("Waiting for PC..");
dreamselec 15:777390eb5afd 494
dreamselec 19:61b21ac4896e 495 lcd->locate(1,0);
dreamselec 19:61b21ac4896e 496 lcd->printf("4:Disconnect");
dreamselec 19:61b21ac4896e 497 D_LEDS_OFF();
dreamselec 19:61b21ac4896e 498 i2cport->write_bit(1,15);
dreamselec 18:44a1c1a30166 499 }