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@10:16ba52f8e025, 2015-11-18 (annotated)

Committer:
dreamselec
Date:
Wed Nov 18 16:09:52 2015 +0000
Revision:
10:16ba52f8e025
Parent:
9:dc8f155b71c8
Child:
11:0fe833f8a1ab
Fine tuneing with the hardware. Logic seems fine. Need to fill in all command functions.

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