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@15:777390eb5afd, 2015-11-22 (annotated)

Committer:
dreamselec
Date:
Sun Nov 22 20:29:08 2015 +0000
Revision:
15:777390eb5afd
Parent:
14:cf2f255b5560
Child:
17:af373246bf80
For some reason cannot assign Commander's command and commandValue properties, to received string. The execution stop not that statement. Noticed using print statements.

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