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@14:cf2f255b5560, 2015-11-22 (annotated)

Committer:
dreamselec
Date:
Sun Nov 22 17:53:36 2015 +0000
Revision:
14:cf2f255b5560
Parent:
11:0fe833f8a1ab
Child:
15:777390eb5afd
Changed sorting algorithm for colour sensor next to top beam.

Added filled some commands rather than just printing.

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 14:cf2f255b5560 102 lcd->locate(1, 0);
dreamselec 14:cf2f255b5560 103 lcd->printf("for block.");
dreamselec 10:16ba52f8e025 104
dreamselec 10:16ba52f8e025 105 i2cport->write_bit(1, 15);
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 14:cf2f255b5560 109 blockInserted = fpga->getBeamValue(Top);
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 14:cf2f255b5560 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 14:cf2f255b5560 132 int canCheckForSize = fpga->checkForBlock();
dreamselec 10:16ba52f8e025 133 int blockSize = 0;
dreamselec 14:cf2f255b5560 134
dreamselec 14:cf2f255b5560 135 while (canCheckForSize == 0) {
dreamselec 14:cf2f255b5560 136 canCheckForSize = fpga->checkForBlock();
dreamselec 14:cf2f255b5560 137 }
dreamselec 14:cf2f255b5560 138
dreamselec 10:16ba52f8e025 139 blockSize = fpga->checkForSize();
dreamselec 10:16ba52f8e025 140 myLED3 = blockSize;
dreamselec 6:98fe30430194 141
dreamselec 14:cf2f255b5560 142 if (blockSize == Block::Small) {
dreamselec 10:16ba52f8e025 143 // detectColour()
dreamselec 10:16ba52f8e025 144 int colourValues[4];
dreamselec 11:0fe833f8a1ab 145 // displayWaitingLine();
dreamselec 11:0fe833f8a1ab 146 rgbSensor.getAllColors(colourValues);
dreamselec 11:0fe833f8a1ab 147 // lcd->cls();
dreamselec 11:0fe833f8a1ab 148 // lcd->locate(0,0);
dreamselec 11:0fe833f8a1ab 149 // lcd->printf("Sorting");
dreamselec 10:16ba52f8e025 150 sortBlock(colourValues);
dreamselec 10:16ba52f8e025 151 }else {
dreamselec 10:16ba52f8e025 152 fpga->moveStoppingServo(Go);
dreamselec 10:16ba52f8e025 153 while(fpga->checkForSize()){}
dreamselec 10:16ba52f8e025 154 fpga->moveStoppingServo(Stop);
dreamselec 10:16ba52f8e025 155 }
dreamselec 10:16ba52f8e025 156
dreamselec 10:16ba52f8e025 157 myLED3 = 0;
dreamselec 10:16ba52f8e025 158 // Re-Attach rx interrupt
dreamselec 10:16ba52f8e025 159 NVIC_EnableIRQ(UART1_IRQn);
dreamselec 10:16ba52f8e025 160 }
dreamselec 10:16ba52f8e025 161 // if (abortOperation == true || connectedToPC == true){ break; }
dreamselec 10:16ba52f8e025 162 }
dreamselec 8:e1da2ae62885 163
dreamselec 10:16ba52f8e025 164 if (selection == 2 || connectedToPC == true) {
dreamselec 10:16ba52f8e025 165 for (;;) {
dreamselec 9:dc8f155b71c8 166 if (!connectedToPC) {
dreamselec 9:dc8f155b71c8 167 i2cport->write_bit(1, 13);
dreamselec 9:dc8f155b71c8 168 // Wait for PC to send '!<pc>connect;' command.
dreamselec 9:dc8f155b71c8 169 lcd->cls();
dreamselec 9:dc8f155b71c8 170 LCDFL();
dreamselec 9:dc8f155b71c8 171 lcd->printf("Waiting for PC...");
dreamselec 9:dc8f155b71c8 172 wait(0.5);
dreamselec 9:dc8f155b71c8 173 D_LEDS_OFF();
dreamselec 9:dc8f155b71c8 174 LCDSL();
dreamselec 9:dc8f155b71c8 175 lcd->printf("4: Main Menu");
dreamselec 9:dc8f155b71c8 176 }
dreamselec 10:16ba52f8e025 177 i2cport->write_bit(1, 15);
dreamselec 10:16ba52f8e025 178 int abortOperation = false;
dreamselec 10:16ba52f8e025 179 while (connectedToPC == false && abortOperation == false) {
dreamselec 10:16ba52f8e025 180 abortOperation = readSwitches() == 4;
dreamselec 10:16ba52f8e025 181 }
dreamselec 14:cf2f255b5560 182
dreamselec 10:16ba52f8e025 183 if (abortOperation == true) {
dreamselec 10:16ba52f8e025 184 D_LEDS_OFF();
dreamselec 10:16ba52f8e025 185 break;
dreamselec 10:16ba52f8e025 186 }
dreamselec 8:e1da2ae62885 187
dreamselec 10:16ba52f8e025 188 while (abortOperation == false && connectedToPC == true) {
dreamselec 10:16ba52f8e025 189 if (runServoTest == true){
dreamselec 10:16ba52f8e025 190 runInServoTestMode();
dreamselec 10:16ba52f8e025 191 lcd->cls();
dreamselec 10:16ba52f8e025 192 lcd->locate(0,0);
dreamselec 10:16ba52f8e025 193 lcd->printf("Connected to PC");
dreamselec 10:16ba52f8e025 194 lcd->locate(1,0);
dreamselec 10:16ba52f8e025 195 lcd->printf("4:Disconnect");
dreamselec 10:16ba52f8e025 196 i2cport->write_bit(1,15);
dreamselec 10:16ba52f8e025 197 }
dreamselec 10:16ba52f8e025 198 abortOperation = readSwitches() == 4;
dreamselec 10:16ba52f8e025 199 }
dreamselec 6:98fe30430194 200
dreamselec 10:16ba52f8e025 201 if (abortOperation == true ) {
dreamselec 10:16ba52f8e025 202 connectedToPC = false;
dreamselec 10:16ba52f8e025 203 D_LEDS_OFF();
dreamselec 10:16ba52f8e025 204 break;
dreamselec 10:16ba52f8e025 205 }
dreamselec 10:16ba52f8e025 206 }
dreamselec 10:16ba52f8e025 207 }
dreamselec 10:16ba52f8e025 208
dreamselec 10:16ba52f8e025 209 }
dreamselec 6:98fe30430194 210 }
dreamselec 6:98fe30430194 211
dreamselec 6:98fe30430194 212 /// Called every-time it receives an char from PC.
dreamselec 10:16ba52f8e025 213 void Rx_interrupt()
dreamselec 10:16ba52f8e025 214 {
dreamselec 10:16ba52f8e025 215 char interruptChar = pc.getc();
dreamselec 10:16ba52f8e025 216 // Uncomment to Echo to USB serial to watch data flow
dreamselec 10:16ba52f8e025 217 // pc.putc(interruptChar);
dreamselec 6:98fe30430194 218
dreamselec 10:16ba52f8e025 219 NVIC_DisableIRQ(UART1_IRQn);
dreamselec 8:e1da2ae62885 220
dreamselec 10:16ba52f8e025 221 if (interruptChar == CommandTypeValue[Query]) {
dreamselec 10:16ba52f8e025 222 commander->decodeCommand(Query);
dreamselec 10:16ba52f8e025 223 } else if (interruptChar == CommandTypeValue[Set]) {
dreamselec 10:16ba52f8e025 224 commander->decodeCommand(Set);
dreamselec 10:16ba52f8e025 225 } else if (interruptChar== CommandTypeValue[Reply]) {
dreamselec 10:16ba52f8e025 226 commander->decodeCommand(Reply);
dreamselec 10:16ba52f8e025 227 }
dreamselec 8:e1da2ae62885 228
dreamselec 10:16ba52f8e025 229 NVIC_EnableIRQ(UART1_IRQn);
dreamselec 6:98fe30430194 230 }
dreamselec 3:843b830ee8bd 231
dreamselec 4:4eebb4de22a7 232 void initInternal()
dreamselec 4:4eebb4de22a7 233 {
dreamselec 10:16ba52f8e025 234 i2cport = new MCP23017(p9, p10, 0x40);
dreamselec 10:16ba52f8e025 235 lcd = new WattBob_TextLCD(i2cport);
dreamselec 10:16ba52f8e025 236 myLED1 = 1;
dreamselec 10:16ba52f8e025 237 BACKLIGHT_ON(i2cport);
dreamselec 10:16ba52f8e025 238 lcd->cls();
dreamselec 10:16ba52f8e025 239 LCDFL();
dreamselec 10:16ba52f8e025 240 lcd->printf("Initilizing...");
dreamselec 10:16ba52f8e025 241 myLED2 = 1;
dreamselec 10:16ba52f8e025 242 return;
dreamselec 3:843b830ee8bd 243 }
dreamselec 3:843b830ee8bd 244
dreamselec 7:b6e31bfdb2af 245 void initPort(int baudRate)
dreamselec 4:4eebb4de22a7 246 {
dreamselec 10:16ba52f8e025 247 myLED3 = 1;
dreamselec 10:16ba52f8e025 248 pc.baud(baudRate);
dreamselec 10:16ba52f8e025 249 pc.format(8, SerialBase::None, gStopBits);
dreamselec 10:16ba52f8e025 250 myLED4 = 1;
dreamselec 10:16ba52f8e025 251 wait (0.1);
dreamselec 10:16ba52f8e025 252 return;
dreamselec 3:843b830ee8bd 253 }
dreamselec 3:843b830ee8bd 254
dreamselec 10:16ba52f8e025 255 void sortBlock(int colourValues[]){
dreamselec 10:16ba52f8e025 256 bool isHazColour[4] = {false, false, false, false };
dreamselec 10:16ba52f8e025 257
dreamselec 10:16ba52f8e025 258 for (int i = 0; i < 4; i++){
dreamselec 10:16ba52f8e025 259 if (colourValues[i] < HazBlock->maxColour.components[i] && colourValues[i] > HazBlock->minColour.components[i]){
dreamselec 10:16ba52f8e025 260 isHazColour[i] = true;
dreamselec 10:16ba52f8e025 261 }
dreamselec 6:98fe30430194 262 }
dreamselec 10:16ba52f8e025 263
dreamselec 10:16ba52f8e025 264 fpga->moveSortingServo(Haz);
dreamselec 10:16ba52f8e025 265 fpga->moveStoppingServo(Go);
dreamselec 10:16ba52f8e025 266 while(fpga->checkForBlock()) {}
dreamselec 10:16ba52f8e025 267 fpga->moveStoppingServo(Stop);
dreamselec 10:16ba52f8e025 268 wait(0.5);
dreamselec 10:16ba52f8e025 269 fpga->moveSortingServo(NonHaz);
dreamselec 10:16ba52f8e025 270 return;
dreamselec 10:16ba52f8e025 271
dreamselec 10:16ba52f8e025 272 bool isHazBlock = false;
dreamselec 10:16ba52f8e025 273
dreamselec 10:16ba52f8e025 274 if (isHazColour[0] && isHazColour[1] && isHazColour[2] && isHazColour[3]){
dreamselec 10:16ba52f8e025 275 fpga->moveSortingServo(Haz);
dreamselec 10:16ba52f8e025 276 fpga->moveStoppingServo(Go);
dreamselec 10:16ba52f8e025 277 while(fpga->checkForBlock()) {}
dreamselec 10:16ba52f8e025 278 fpga->moveStoppingServo(Stop);
dreamselec 10:16ba52f8e025 279 fpga->moveSortingServo(NonHaz);
dreamselec 10:16ba52f8e025 280 }else {
dreamselec 10:16ba52f8e025 281 fpga->moveStoppingServo(Go);
dreamselec 10:16ba52f8e025 282 while(fpga->checkForBlock()) {}
dreamselec 10:16ba52f8e025 283 fpga->moveStoppingServo(Stop);
dreamselec 10:16ba52f8e025 284 }
dreamselec 3:843b830ee8bd 285 }
dreamselec 3:843b830ee8bd 286
dreamselec 6:98fe30430194 287 void printPCDetectedText()
dreamselec 4:4eebb4de22a7 288 {
dreamselec 10:16ba52f8e025 289 lcd->cls();
dreamselec 10:16ba52f8e025 290 LCDFL();
dreamselec 10:16ba52f8e025 291 lcd->printf("Detected PC.");
dreamselec 10:16ba52f8e025 292 LCDSL();
dreamselec 10:16ba52f8e025 293 lcd->printf("Connecting");
dreamselec 10:16ba52f8e025 294 initPort();
dreamselec 10:16ba52f8e025 295 }
dreamselec 10:16ba52f8e025 296
dreamselec 10:16ba52f8e025 297 bool displayAbortDialog()
dreamselec 10:16ba52f8e025 298 {
dreamselec 10:16ba52f8e025 299 while (i2cport->read_bit(11) == 1) {}
dreamselec 10:16ba52f8e025 300 i2cport->write_bit(1, 12);
dreamselec 10:16ba52f8e025 301
dreamselec 10:16ba52f8e025 302 lcd->cls();
dreamselec 10:16ba52f8e025 303 LCDFL();
dreamselec 10:16ba52f8e025 304 lcd->printf("Abort?");
dreamselec 10:16ba52f8e025 305 LCDSL();
dreamselec 10:16ba52f8e025 306 lcd->printf("1:YES, 2,3,4:NO");
dreamselec 10:16ba52f8e025 307 int reply = 0;
dreamselec 10:16ba52f8e025 308 do {
dreamselec 10:16ba52f8e025 309 reply = readSwitches();
dreamselec 10:16ba52f8e025 310 } while(reply == 0);
dreamselec 10:16ba52f8e025 311
dreamselec 10:16ba52f8e025 312 D_LEDS_OFF();
dreamselec 10:16ba52f8e025 313 if (reply == 1) {
dreamselec 10:16ba52f8e025 314 return true;
dreamselec 10:16ba52f8e025 315 } else {
dreamselec 10:16ba52f8e025 316 return false;
dreamselec 10:16ba52f8e025 317 }
dreamselec 3:843b830ee8bd 318 }
dreamselec 3:843b830ee8bd 319
dreamselec 10:16ba52f8e025 320 void printServoInfoOnLCD(){
dreamselec 10:16ba52f8e025 321 lcd->cls();
dreamselec 10:16ba52f8e025 322 lcd->locate(0,0);
dreamselec 10:16ba52f8e025 323 lcd->printf("1: Stop servo:%i", fpga->stoppingServoPosition);
dreamselec 10:16ba52f8e025 324 lcd->locate(1,0);
dreamselec 10:16ba52f8e025 325 lcd->printf("2: Sort servo:%i", fpga->sortingServoPosition);
dreamselec 10:16ba52f8e025 326 }
dreamselec 10:16ba52f8e025 327
dreamselec 10:16ba52f8e025 328 void runInServoTestMode(){
dreamselec 10:16ba52f8e025 329 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 330 printServoInfoOnLCD();
dreamselec 10:16ba52f8e025 331 int finished = false;
dreamselec 9:dc8f155b71c8 332
dreamselec 10:16ba52f8e025 333 i2cport->write_bit(1, 12);
dreamselec 10:16ba52f8e025 334 i2cport->write_bit(1, 13);
dreamselec 10:16ba52f8e025 335 i2cport->write_bit(1, 15);
dreamselec 10:16ba52f8e025 336 int button = 0;
dreamselec 10:16ba52f8e025 337 do {
dreamselec 10:16ba52f8e025 338 button = readSwitches();
dreamselec 10:16ba52f8e025 339 if (button == 1){
dreamselec 10:16ba52f8e025 340 fpga->toggleStoppingServo();
dreamselec 10:16ba52f8e025 341 printServoInfoOnLCD();
dreamselec 10:16ba52f8e025 342 wait(0.5);
dreamselec 10:16ba52f8e025 343 }else if (button == 2) {
dreamselec 10:16ba52f8e025 344 fpga->toggleSortingServo();
dreamselec 10:16ba52f8e025 345 printServoInfoOnLCD();
dreamselec 10:16ba52f8e025 346 wait(0.5);
dreamselec 10:16ba52f8e025 347 }
dreamselec 10:16ba52f8e025 348 finished = button == 4;
dreamselec 10:16ba52f8e025 349 button = 0;
dreamselec 10:16ba52f8e025 350 } while (finished == false);
dreamselec 10:16ba52f8e025 351 runServoTest = false;
dreamselec 10:16ba52f8e025 352 D_LEDS_OFF();
dreamselec 10:16ba52f8e025 353 lcd->cls();
dreamselec 10:16ba52f8e025 354 lcd->locate(0,0);
dreamselec 10:16ba52f8e025 355 lcd->printf("Done servo test");
dreamselec 10:16ba52f8e025 356 wait(0.5);
dreamselec 10:16ba52f8e025 357 lcd->cls();
dreamselec 10:16ba52f8e025 358 return;
dreamselec 2:7a55cb10259f 359 }