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@19:61b21ac4896e, 2015-11-24 (annotated)

Committer:
dreamselec
Date:
Tue Nov 24 01:38:46 2015 +0000
Revision:
19:61b21ac4896e
Parent:
18:44a1c1a30166
Child:
20:4e0f0944f28f
Modes testing working. Need to work on setting new has block similar to other program. Added tags.

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