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@17:af373246bf80, 2015-11-23 (annotated)

Committer:
dreamselec
Date:
Mon Nov 23 23:13:39 2015 +0000
Revision:
17:af373246bf80
Parent:
15:777390eb5afd
Child:
18:44a1c1a30166
Added tags for parsing on PC. Added more commands.

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 17:af373246bf80 319 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 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 17:af373246bf80 340 } while (finished == false && runServoTest != false);
dreamselec 17:af373246bf80 341
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 }