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@11:0fe833f8a1ab, 2015-11-21 (annotated)

Committer:
dreamselec
Date:
Sat Nov 21 18:52:07 2015 +0000
Revision:
11:0fe833f8a1ab
Parent:
10:16ba52f8e025
Child:
14:cf2f255b5560
Testing with hardware, working fine. Implemented servo test mode. Need to change sorting algorithm because now we're reading colour right after top beam break, and size after that.

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 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 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 }