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@18:44a1c1a30166, 2015-11-24 (annotated)

Committer:
dreamselec
Date:
Tue Nov 24 00:24:39 2015 +0000
Revision:
18:44a1c1a30166
Parent:
17:af373246bf80
Child:
19:61b21ac4896e
Implemented new algorithm, now supports different modes on PC.

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