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@26:bbcc25418ffa, 2015-11-30 (annotated)

Committer:
dreamselec
Date:
Mon Nov 30 21:29:51 2015 +0000
Revision:
26:bbcc25418ffa
Parent:
25:792540d69c49
Child:
27:2cb1bdb7ae3d
Small changes in program so it doesn't get stuck and PC receives correct commands. Also commented out hard-coded red values. currently use a buffer error variables which can be set to anything.

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 10:16ba52f8e025 27 TCS3472_I2C rgbSensor(p28, p27);
dreamselec 2:7a55cb10259f 28 Serial pc(USBTX, USBRX);
dreamselec 8:e1da2ae62885 29 uint8_t rxBuffer[kSmallBufferSize + 1];
dreamselec 8:e1da2ae62885 30 int rxIndex = 0;
dreamselec 21:60c4fe94d79c 31 float percentageError[3];
dreamselec 21:60c4fe94d79c 32 float adjustedValues[3];
dreamselec 2:7a55cb10259f 33
dreamselec 6:98fe30430194 34 Commander _commander = Commander();
dreamselec 6:98fe30430194 35 Commander *commander = &_commander;
dreamselec 6:98fe30430194 36 FPGA _fpga = FPGA();
dreamselec 9:dc8f155b71c8 37 extern FPGA *fpga = &_fpga;
dreamselec 6:98fe30430194 38
dreamselec 20:4e0f0944f28f 39 Block defualtHazBlock = Block();
dreamselec 6:98fe30430194 40 extern Block _HazBlock;
dreamselec 6:98fe30430194 41 Block *HazBlock = &_HazBlock;
dreamselec 6:98fe30430194 42
dreamselec 18:44a1c1a30166 43 extern PCModes currentMode;
dreamselec 18:44a1c1a30166 44
dreamselec 6:98fe30430194 45 void initInternal();
dreamselec 7:b6e31bfdb2af 46 void initPort(int baudRate=kDefaultBaudRate);
dreamselec 6:98fe30430194 47 void printPCDetectedText();
dreamselec 6:98fe30430194 48 void Rx_interrupt();
dreamselec 18:44a1c1a30166 49 bool checkColour(int colourValues[]);
dreamselec 10:16ba52f8e025 50 void runInServoTestMode();
dreamselec 15:777390eb5afd 51 void displayWaitingLine();
dreamselec 15:777390eb5afd 52 void displayPCStatus();
dreamselec 18:44a1c1a30166 53 bool waitForBlock();
dreamselec 18:44a1c1a30166 54 void sortBlock();
dreamselec 23:db91aaa43a9e 55 void runInBreakBeamTestMode();
dreamselec 23:db91aaa43a9e 56 void turnOffTopLEDs();
dreamselec 23:db91aaa43a9e 57 void turnOffBottomLEDs();
dreamselec 25:792540d69c49 58 void runInColourSensorTestMode();
dreamselec 6:98fe30430194 59
dreamselec 6:98fe30430194 60 int main()
dreamselec 6:98fe30430194 61 {
dreamselec 23:db91aaa43a9e 62 initInternal();
dreamselec 23:db91aaa43a9e 63 initPort();
dreamselec 23:db91aaa43a9e 64 srand((unsigned)time(NULL));
dreamselec 23:db91aaa43a9e 65 U_LEDS_OFF();
dreamselec 23:db91aaa43a9e 66 lcd->cls();
dreamselec 15:777390eb5afd 67
dreamselec 23:db91aaa43a9e 68 rgbSensor.enablePowerAndRGBC();
dreamselec 25:792540d69c49 69 rgbSensor.setIntegrationTime(gIntegrationTime);
dreamselec 6:98fe30430194 70
dreamselec 23:db91aaa43a9e 71 // Create a serial intereput for RxIrq so when PC is connected it sends '$' to tell MBED it's there.
dreamselec 23:db91aaa43a9e 72 // https://developer.mbed.org/cookbook/Serial-Interrupts
dreamselec 23:db91aaa43a9e 73 pc.attach(&Rx_interrupt, Serial::RxIrq);
dreamselec 6:98fe30430194 74
dreamselec 26:bbcc25418ffa 75 DefaultHazBlock();
dreamselec 26:bbcc25418ffa 76 fpga->moveStoppingServo(Stop);
dreamselec 26:bbcc25418ffa 77 fpga->moveSortingServo(NonHaz);
dreamselec 26:bbcc25418ffa 78 pc.printf(":<pc>connect;");
dreamselec 26:bbcc25418ffa 79
dreamselec 23:db91aaa43a9e 80 for (;;) {
dreamselec 26:bbcc25418ffa 81 if (connectedToPC == false){
dreamselec 23:db91aaa43a9e 82 lcd->cls();
dreamselec 23:db91aaa43a9e 83 i2cport->write_bit(1, 12);
dreamselec 23:db91aaa43a9e 84 lcd->printf("1: Start sorting.");
dreamselec 23:db91aaa43a9e 85 LCDSL();
dreamselec 23:db91aaa43a9e 86 i2cport->write_bit(1,13);
dreamselec 23:db91aaa43a9e 87 lcd->printf("2: Connect to PC");
dreamselec 26:bbcc25418ffa 88 }
dreamselec 24:02c61793f90b 89
dreamselec 24:02c61793f90b 90 // wait(0.03);
dreamselec 10:16ba52f8e025 91
dreamselec 23:db91aaa43a9e 92 int selection = 0;
dreamselec 23:db91aaa43a9e 93 do {
dreamselec 23:db91aaa43a9e 94 myLED4 = selection;
dreamselec 23:db91aaa43a9e 95 selection = readSwitches();
dreamselec 23:db91aaa43a9e 96 } while (selection != 1 && selection != 2 && connectedToPC == false);
dreamselec 23:db91aaa43a9e 97 D_LEDS_OFF();
dreamselec 23:db91aaa43a9e 98 if (selection == 1) {
dreamselec 23:db91aaa43a9e 99 // User selected op 1: Start sorting autonomously.
dreamselec 23:db91aaa43a9e 100 i2cport->write_bit(1, 12);
dreamselec 23:db91aaa43a9e 101 lcd->cls();
dreamselec 23:db91aaa43a9e 102 LCDFL();
dreamselec 23:db91aaa43a9e 103 lcd->printf("Starting sorting");
dreamselec 23:db91aaa43a9e 104 wait(0.5);
dreamselec 23:db91aaa43a9e 105 D_LEDS_OFF();
dreamselec 19:61b21ac4896e 106
dreamselec 23:db91aaa43a9e 107 for(;;) {
dreamselec 23:db91aaa43a9e 108 displayWaitingLine();
dreamselec 23:db91aaa43a9e 109 lcd->printf("for block.");
dreamselec 10:16ba52f8e025 110
dreamselec 23:db91aaa43a9e 111 i2cport->write_bit(1, 15);
dreamselec 10:16ba52f8e025 112
dreamselec 23:db91aaa43a9e 113 // Break and return to main menu i.e. Start Op, or Connect to PC.
dreamselec 23:db91aaa43a9e 114 if (waitForBlock() == false) {
dreamselec 23:db91aaa43a9e 115 D_LEDS_OFF();
dreamselec 23:db91aaa43a9e 116 break;
dreamselec 23:db91aaa43a9e 117 } else {
dreamselec 23:db91aaa43a9e 118 sortBlock();
dreamselec 23:db91aaa43a9e 119 }
dreamselec 23:db91aaa43a9e 120 }
dreamselec 23:db91aaa43a9e 121 }
dreamselec 23:db91aaa43a9e 122
dreamselec 23:db91aaa43a9e 123 if (selection == 2 || connectedToPC == true) {
dreamselec 23:db91aaa43a9e 124 for (;;) {
dreamselec 23:db91aaa43a9e 125 displayPCStatus();
dreamselec 10:16ba52f8e025 126
dreamselec 23:db91aaa43a9e 127 i2cport->write_bit(1, 15);
dreamselec 23:db91aaa43a9e 128 int abortOperation = false;
dreamselec 23:db91aaa43a9e 129 if (connectedToPC == false)
dreamselec 23:db91aaa43a9e 130 pc.printf(":<pc>connect;");
dreamselec 23:db91aaa43a9e 131 while (connectedToPC == false && abortOperation == false) {
dreamselec 23:db91aaa43a9e 132 abortOperation = readSwitches() == 4;
dreamselec 23:db91aaa43a9e 133 }
dreamselec 19:61b21ac4896e 134
dreamselec 23:db91aaa43a9e 135 if (abortOperation == true) {
dreamselec 23:db91aaa43a9e 136 D_LEDS_OFF();
dreamselec 23:db91aaa43a9e 137 break;
dreamselec 23:db91aaa43a9e 138 }
dreamselec 18:44a1c1a30166 139
dreamselec 23:db91aaa43a9e 140 displayPCStatus();
dreamselec 15:777390eb5afd 141
dreamselec 23:db91aaa43a9e 142 while (abortOperation == false && connectedToPC == true) {
dreamselec 23:db91aaa43a9e 143 if (currentMode == Maintanence) {
dreamselec 24:02c61793f90b 144 displayPCStatus();
dreamselec 24:02c61793f90b 145 while (currentMode == Maintanence){
dreamselec 23:db91aaa43a9e 146 if (runServoTest == true)
dreamselec 23:db91aaa43a9e 147 runInServoTestMode();
dreamselec 23:db91aaa43a9e 148 else if (runBreakBeamTest == true)
dreamselec 23:db91aaa43a9e 149 runInBreakBeamTestMode();
dreamselec 25:792540d69c49 150 else if (runColourSensorTest == true)
dreamselec 25:792540d69c49 151 runInColourSensorTestMode();
dreamselec 26:bbcc25418ffa 152 if (i2cport->read_bit(11) == 1){
dreamselec 26:bbcc25418ffa 153 if (displayAbortDialog() == true){
dreamselec 26:bbcc25418ffa 154 currentMode = None;
dreamselec 26:bbcc25418ffa 155 pc.printf(":<mbed>mode=none;");
dreamselec 26:bbcc25418ffa 156 }else {
dreamselec 26:bbcc25418ffa 157 displayPCStatus();
dreamselec 26:bbcc25418ffa 158 }
dreamselec 26:bbcc25418ffa 159 }
dreamselec 24:02c61793f90b 160 }
dreamselec 23:db91aaa43a9e 161 } else if (currentMode == Normal) {
dreamselec 23:db91aaa43a9e 162 displayPCStatus();
dreamselec 23:db91aaa43a9e 163 while (currentMode == Normal) {
dreamselec 23:db91aaa43a9e 164 if (currentState == Pause) {
dreamselec 23:db91aaa43a9e 165 lcd->cls();
dreamselec 23:db91aaa43a9e 166 lcd->locate(0,0);
dreamselec 23:db91aaa43a9e 167 lcd->printf("Sorting Paused.");
dreamselec 23:db91aaa43a9e 168 lcd->locate(1,0);
dreamselec 23:db91aaa43a9e 169 lcd->printf("1: Start");
dreamselec 23:db91aaa43a9e 170 int button = 0;
dreamselec 23:db91aaa43a9e 171 i2cport->write_bit(1, 12);
dreamselec 23:db91aaa43a9e 172 i2cport->write_bit(1, 15);
dreamselec 23:db91aaa43a9e 173 while (currentState == Pause && currentMode == Normal) {
dreamselec 23:db91aaa43a9e 174 button = readSwitches();
dreamselec 23:db91aaa43a9e 175 if (button == 1) {
dreamselec 26:bbcc25418ffa 176 pc.printf(":<mbed>sort=start;");
dreamselec 23:db91aaa43a9e 177 currentState = Start;
dreamselec 23:db91aaa43a9e 178 } else if (button == 4) {
dreamselec 23:db91aaa43a9e 179 currentMode = None;
dreamselec 26:bbcc25418ffa 180 pc.printf(":<mbed>mode=none;");
dreamselec 23:db91aaa43a9e 181 // goto setModeNone;
dreamselec 23:db91aaa43a9e 182 break;
dreamselec 23:db91aaa43a9e 183 }
dreamselec 23:db91aaa43a9e 184 }
dreamselec 23:db91aaa43a9e 185 }
dreamselec 23:db91aaa43a9e 186 if (currentState == Start) {
dreamselec 23:db91aaa43a9e 187 lcd->cls();
dreamselec 23:db91aaa43a9e 188 lcd->locate(0,0);
dreamselec 23:db91aaa43a9e 189 lcd->printf("Sorting mode...");
dreamselec 23:db91aaa43a9e 190 while (currentState == Start && currentMode == Normal) {
dreamselec 23:db91aaa43a9e 191 if (waitForBlock() == false) {
dreamselec 26:bbcc25418ffa 192 if (connectedToPC == true && currentState != Pause) {
dreamselec 23:db91aaa43a9e 193 // TODO: Tell PC to update UI if aborted from MBED.
dreamselec 26:bbcc25418ffa 194 pc.printf(":<mbed>sort=pause;");
dreamselec 23:db91aaa43a9e 195 currentState = Pause;
dreamselec 23:db91aaa43a9e 196 continue;
dreamselec 23:db91aaa43a9e 197 } else if (connectedToPC == false) {
dreamselec 23:db91aaa43a9e 198 currentMode = None;
dreamselec 23:db91aaa43a9e 199 abortOperation = true;
dreamselec 23:db91aaa43a9e 200 }
dreamselec 23:db91aaa43a9e 201 } else {
dreamselec 23:db91aaa43a9e 202 sortBlock();
dreamselec 23:db91aaa43a9e 203 }
dreamselec 23:db91aaa43a9e 204 }
dreamselec 23:db91aaa43a9e 205 }
dreamselec 23:db91aaa43a9e 206 }
dreamselec 23:db91aaa43a9e 207 } else if (currentMode == None) {
dreamselec 23:db91aaa43a9e 208 setModeNone:
dreamselec 23:db91aaa43a9e 209 D_LEDS_OFF();
dreamselec 23:db91aaa43a9e 210 i2cport->write_bit(1,15);
dreamselec 23:db91aaa43a9e 211 displayPCStatus();
dreamselec 23:db91aaa43a9e 212 while (currentMode == None && abortOperation == false && connectedToPC == true) {
dreamselec 23:db91aaa43a9e 213 if (i2cport->read_bit(11)) {
dreamselec 23:db91aaa43a9e 214 abortOperation = displayAbortDialog();
dreamselec 23:db91aaa43a9e 215 // Cancel the Abort
dreamselec 23:db91aaa43a9e 216 if (abortOperation == false) {
dreamselec 23:db91aaa43a9e 217 displayPCStatus();
dreamselec 23:db91aaa43a9e 218 i2cport->write_bit(1, 15);
dreamselec 23:db91aaa43a9e 219 }
dreamselec 23:db91aaa43a9e 220 }
dreamselec 23:db91aaa43a9e 221 }
dreamselec 23:db91aaa43a9e 222 }
dreamselec 23:db91aaa43a9e 223 }
dreamselec 15:777390eb5afd 224
dreamselec 23:db91aaa43a9e 225 if (abortOperation == true ) {
dreamselec 23:db91aaa43a9e 226 connectedToPC = false;
dreamselec 23:db91aaa43a9e 227 D_LEDS_OFF();
dreamselec 23:db91aaa43a9e 228 break;
dreamselec 23:db91aaa43a9e 229 }
dreamselec 23:db91aaa43a9e 230 }
dreamselec 23:db91aaa43a9e 231 }
dreamselec 19:61b21ac4896e 232
dreamselec 23:db91aaa43a9e 233 }
dreamselec 18:44a1c1a30166 234 }
dreamselec 8:e1da2ae62885 235
dreamselec 18:44a1c1a30166 236 // Waits until detects block.
dreamselec 18:44a1c1a30166 237 // true if block detected, false if cancelled/connected to PC.
dreamselec 19:61b21ac4896e 238 bool waitForBlock()
dreamselec 19:61b21ac4896e 239 {
dreamselec 23:db91aaa43a9e 240 myLED4 = 0;
dreamselec 23:db91aaa43a9e 241 myLED1 = 1;
dreamselec 23:db91aaa43a9e 242 if (connectedToPC == false) {
dreamselec 23:db91aaa43a9e 243 bool abortOperation = false;
dreamselec 23:db91aaa43a9e 244 int blockInserted = 0;
dreamselec 23:db91aaa43a9e 245 // Wait until a block is breaking the beam, or button 4 is pressed to abort.
dreamselec 23:db91aaa43a9e 246 do {
dreamselec 23:db91aaa43a9e 247 blockInserted = fpga->getBeamValue(Top);
dreamselec 23:db91aaa43a9e 248 myLED4 = blockInserted;
dreamselec 23:db91aaa43a9e 249 if (i2cport->read_bit(11)) {
dreamselec 23:db91aaa43a9e 250 abortOperation = displayAbortDialog();
dreamselec 23:db91aaa43a9e 251 // Cancel the Abort
dreamselec 23:db91aaa43a9e 252 if (abortOperation == false) {
dreamselec 23:db91aaa43a9e 253 displayWaitingLine();
dreamselec 23:db91aaa43a9e 254 lcd->printf("for block");
dreamselec 23:db91aaa43a9e 255 i2cport->write_bit(1, 15);
dreamselec 23:db91aaa43a9e 256 }
dreamselec 23:db91aaa43a9e 257 }
dreamselec 23:db91aaa43a9e 258 } while (abortOperation == false && blockInserted != 1 && connectedToPC == false);
dreamselec 18:44a1c1a30166 259
dreamselec 23:db91aaa43a9e 260 if (abortOperation == true || connectedToPC == true)
dreamselec 23:db91aaa43a9e 261 return false;
dreamselec 23:db91aaa43a9e 262 else
dreamselec 23:db91aaa43a9e 263 return true;
dreamselec 23:db91aaa43a9e 264 } else if (connectedToPC == true) {
dreamselec 23:db91aaa43a9e 265 bool abortOperation = false;
dreamselec 23:db91aaa43a9e 266 int blockInserted = 0;
dreamselec 23:db91aaa43a9e 267 // Wait until a block is breaking the beam, or button 4 is pressed to abort.
dreamselec 23:db91aaa43a9e 268 do {
dreamselec 23:db91aaa43a9e 269 blockInserted = fpga->getBeamValue(Top);
dreamselec 23:db91aaa43a9e 270 myLED4 = blockInserted;
dreamselec 23:db91aaa43a9e 271 if (i2cport->read_bit(11)) {
dreamselec 23:db91aaa43a9e 272 abortOperation = displayAbortDialog();
dreamselec 23:db91aaa43a9e 273 // Cancel the Abort
dreamselec 23:db91aaa43a9e 274 if (abortOperation == false) {
dreamselec 23:db91aaa43a9e 275 displayWaitingLine();
dreamselec 23:db91aaa43a9e 276 lcd->printf("for block");
dreamselec 23:db91aaa43a9e 277 i2cport->write_bit(1, 15);
dreamselec 23:db91aaa43a9e 278 }
dreamselec 23:db91aaa43a9e 279 }
dreamselec 23:db91aaa43a9e 280 } while (abortOperation == false && blockInserted != 1 && connectedToPC == true && currentState == Start && currentMode == Normal);
dreamselec 19:61b21ac4896e 281
dreamselec 23:db91aaa43a9e 282 if (abortOperation == true || connectedToPC == false || currentState == Pause || currentMode == Maintanence)
dreamselec 23:db91aaa43a9e 283 return false;
dreamselec 23:db91aaa43a9e 284 else
dreamselec 23:db91aaa43a9e 285 return true;
dreamselec 23:db91aaa43a9e 286 }
dreamselec 26:bbcc25418ffa 287 return false;
dreamselec 18:44a1c1a30166 288 }
dreamselec 14:cf2f255b5560 289
dreamselec 19:61b21ac4896e 290 void sortBlock()
dreamselec 19:61b21ac4896e 291 {
dreamselec 23:db91aaa43a9e 292 myLED1 = 0;
dreamselec 23:db91aaa43a9e 293 myLED2 = 1;
dreamselec 23:db91aaa43a9e 294 // Cannot Abort any longer. Block is inserted.
dreamselec 23:db91aaa43a9e 295 // Detach rx interrupt until block processed.
dreamselec 23:db91aaa43a9e 296 NVIC_DisableIRQ(UART1_IRQn);
dreamselec 23:db91aaa43a9e 297 fpga->moveSortingServo(Haz);
dreamselec 23:db91aaa43a9e 298 fpga->moveStoppingServo(Go);
dreamselec 21:60c4fe94d79c 299
dreamselec 23:db91aaa43a9e 300 int colourValues[4];
dreamselec 23:db91aaa43a9e 301 int averageColourValues[4] = {0, 0, 0, 0};
dreamselec 23:db91aaa43a9e 302 for (int i = 0; i < 3; i++){
dreamselec 23:db91aaa43a9e 303 rgbSensor.getAllColors(colourValues);
dreamselec 23:db91aaa43a9e 304 for (int j = 0; j < 4; j++){
dreamselec 23:db91aaa43a9e 305 averageColourValues[j] += colourValues[j];
dreamselec 23:db91aaa43a9e 306 }
dreamselec 23:db91aaa43a9e 307 }
dreamselec 23:db91aaa43a9e 308 for (int i = 0; i < 4; i++){
dreamselec 23:db91aaa43a9e 309 averageColourValues[i] = averageColourValues[i] / 3;
dreamselec 23:db91aaa43a9e 310 }
dreamselec 18:44a1c1a30166 311
dreamselec 23:db91aaa43a9e 312 bool haz = false;
dreamselec 23:db91aaa43a9e 313 haz = checkColour(colourValues);
dreamselec 23:db91aaa43a9e 314
dreamselec 23:db91aaa43a9e 315 if (!haz) {
dreamselec 23:db91aaa43a9e 316 fpga->moveSortingServo(NonHaz);
dreamselec 23:db91aaa43a9e 317 }
dreamselec 22:993821a4c396 318 int blockSize;
dreamselec 23:db91aaa43a9e 319 while (fpga->checkForBlock() == 0) { }
dreamselec 23:db91aaa43a9e 320 blockSize = fpga->checkForSize();
dreamselec 23:db91aaa43a9e 321 if (blockSize == HazBlock->size && haz) {
dreamselec 23:db91aaa43a9e 322 // fpga->moveSortingServo(Haz);
dreamselec 23:db91aaa43a9e 323 // fpga->moveStoppingServo(Go);
dreamselec 23:db91aaa43a9e 324 // blockSize = HazBlock->size;
dreamselec 23:db91aaa43a9e 325 while(fpga->getBeamValue(Bottom) == 1) {}
dreamselec 23:db91aaa43a9e 326 wait(kServoWait);
dreamselec 23:db91aaa43a9e 327 fpga->moveStoppingServo(Stop);
dreamselec 23:db91aaa43a9e 328 } else {
dreamselec 23:db91aaa43a9e 329 haz = false;
dreamselec 23:db91aaa43a9e 330 }
dreamselec 23:db91aaa43a9e 331 fpga->moveSortingServo(NonHaz);
dreamselec 23:db91aaa43a9e 332 while(fpga->checkForSize()) {}
dreamselec 20:4e0f0944f28f 333
dreamselec 23:db91aaa43a9e 334 if (connectedToPC) {
dreamselec 23:db91aaa43a9e 335 for (int i = 0; i < 3; i++) {
dreamselec 23:db91aaa43a9e 336 pc.printf("DEBUG:Percentage Error: %.5f.\n", percentageError[i]);
dreamselec 23:db91aaa43a9e 337 if ((percentageError[i] < 0 && std::abs(percentageError[i]) < kMinError[i] * 2) || percentageError[i] == 0 || (percentageError[i] > 0 && percentageError[i] < kMaxError[i] * 2))
dreamselec 26:bbcc25418ffa 338 pc.printf("DEBUG:%i Pass.\n", i);
dreamselec 23:db91aaa43a9e 339 }
dreamselec 23:db91aaa43a9e 340 pc.printf("BLOCK:Size:%i,Red:%.3f,Green:%.3f,Blue:%.3f,Haz:%i, Offsetred:%.3f, Offsetgreen:%.3f, Offsetblue:%.3f;", blockSize, adjustedValues[0], adjustedValues[1], adjustedValues[2], haz, percentageError[0], percentageError[1], percentageError[2]);
dreamselec 23:db91aaa43a9e 341 // pc.printf("VALUE:Size:%i,Red:%i,Green:%i,Blue:%i,Clear:%i\n:VALUE", blockSize, colourValues[0], colourValues[1], colourValues[2], colourValues[3], haz);
dreamselec 23:db91aaa43a9e 342 }
dreamselec 18:44a1c1a30166 343
dreamselec 23:db91aaa43a9e 344 // Re-Attach rx interrupt
dreamselec 23:db91aaa43a9e 345 NVIC_EnableIRQ(UART1_IRQn);
dreamselec 23:db91aaa43a9e 346 myLED3 = 0;
dreamselec 23:db91aaa43a9e 347 myLED4 = 1;
dreamselec 23:db91aaa43a9e 348 return;
dreamselec 6:98fe30430194 349 }
dreamselec 6:98fe30430194 350
dreamselec 6:98fe30430194 351 /// Called every-time it receives an char from PC.
dreamselec 10:16ba52f8e025 352 void Rx_interrupt()
dreamselec 10:16ba52f8e025 353 {
dreamselec 23:db91aaa43a9e 354 char interruptChar = pc.getc();
dreamselec 23:db91aaa43a9e 355 // Uncomment to Echo to USB serial to watch data flow
dreamselec 23:db91aaa43a9e 356 // pc.putc(interruptChar);
dreamselec 6:98fe30430194 357
dreamselec 23:db91aaa43a9e 358 NVIC_DisableIRQ(UART1_IRQn);
dreamselec 8:e1da2ae62885 359
dreamselec 23:db91aaa43a9e 360 if (interruptChar == CommandTypeValue[Query]) {
dreamselec 23:db91aaa43a9e 361 commander->decodeCommand(Query);
dreamselec 23:db91aaa43a9e 362 } else if (interruptChar == CommandTypeValue[Set]) {
dreamselec 23:db91aaa43a9e 363 commander->decodeCommand(Set);
dreamselec 23:db91aaa43a9e 364 } else if (interruptChar== CommandTypeValue[Reply]) {
dreamselec 23:db91aaa43a9e 365 commander->decodeCommand(Reply);
dreamselec 23:db91aaa43a9e 366 }
dreamselec 8:e1da2ae62885 367
dreamselec 23:db91aaa43a9e 368 NVIC_EnableIRQ(UART1_IRQn);
dreamselec 6:98fe30430194 369 }
dreamselec 3:843b830ee8bd 370
dreamselec 4:4eebb4de22a7 371 void initInternal()
dreamselec 4:4eebb4de22a7 372 {
dreamselec 23:db91aaa43a9e 373 myLED1 = 1;
dreamselec 23:db91aaa43a9e 374 i2cport = new MCP23017(p9, p10, 0x40);
dreamselec 23:db91aaa43a9e 375 lcd = new WattBob_TextLCD(i2cport);
dreamselec 23:db91aaa43a9e 376 BACKLIGHT_ON(i2cport);
dreamselec 23:db91aaa43a9e 377 lcd->cls();
dreamselec 23:db91aaa43a9e 378 lcd->locate(0,0);
dreamselec 23:db91aaa43a9e 379 lcd->printf("Initilizing...");
dreamselec 23:db91aaa43a9e 380 DefaultHazBlock();
dreamselec 23:db91aaa43a9e 381 myLED2 = 1;
dreamselec 23:db91aaa43a9e 382 return;
dreamselec 3:843b830ee8bd 383 }
dreamselec 3:843b830ee8bd 384
dreamselec 7:b6e31bfdb2af 385 void initPort(int baudRate)
dreamselec 4:4eebb4de22a7 386 {
dreamselec 23:db91aaa43a9e 387 myLED3 = 1;
dreamselec 23:db91aaa43a9e 388 pc.baud(baudRate);
dreamselec 23:db91aaa43a9e 389 pc.format(8, SerialBase::None, gStopBits);
dreamselec 23:db91aaa43a9e 390 myLED4 = 1;
dreamselec 23:db91aaa43a9e 391 wait (0.1);
dreamselec 23:db91aaa43a9e 392 return;
dreamselec 3:843b830ee8bd 393 }
dreamselec 3:843b830ee8bd 394
dreamselec 18:44a1c1a30166 395 bool checkColour(int colourValues[])
dreamselec 15:777390eb5afd 396 {
dreamselec 23:db91aaa43a9e 397 myLED2 = 0;
dreamselec 23:db91aaa43a9e 398 myLED3 = 1;
dreamselec 23:db91aaa43a9e 399 bool isHazColour[3] = {false, false, false };
dreamselec 23:db91aaa43a9e 400 memset(adjustedValues, 0, sizeof(adjustedValues));
dreamselec 23:db91aaa43a9e 401 memset(percentageError, 0, sizeof(percentageError));
dreamselec 15:777390eb5afd 402
dreamselec 26:bbcc25418ffa 403 /* Harcoded working
dreamselec 23:db91aaa43a9e 404 for (int i = 0; i < 3; i++) {
dreamselec 23:db91aaa43a9e 405 adjustedValues[i] = (float)colourValues[i]/(float)colourValues[3];
dreamselec 23:db91aaa43a9e 406 percentageError[i] = (adjustedValues[i] - kAverageRedBlock[i]) / kAverageRedBlock[i];
dreamselec 20:4e0f0944f28f 407
dreamselec 23:db91aaa43a9e 408 if ((percentageError[i] < 0 && std::abs(percentageError[i]) < kMinError[i] * 2) || percentageError[i] == 0 || (percentageError[i] > 0 && percentageError[i] < kMaxError[i] * 2)) {
dreamselec 23:db91aaa43a9e 409 isHazColour[i] = true;
dreamselec 23:db91aaa43a9e 410 }
dreamselec 23:db91aaa43a9e 411 }
dreamselec 26:bbcc25418ffa 412 */
dreamselec 26:bbcc25418ffa 413 for (int i = 0; i < 3; i++) {
dreamselec 26:bbcc25418ffa 414 adjustedValues[i] = (float)colourValues[i]/(float)colourValues[3];
dreamselec 26:bbcc25418ffa 415 percentageError[i] = (adjustedValues[i] - kAverageRedBlock[i]) / kAverageRedBlock[i];
dreamselec 15:777390eb5afd 416
dreamselec 26:bbcc25418ffa 417 if ((percentageError[i] < 0 && std::abs(percentageError[i]) < currentMinError[i] * errorMultiplier) || percentageError[i] == 0 || (percentageError[i] > 0 && percentageError[i] < currentMaxError[i] * errorMultiplier)) {
dreamselec 26:bbcc25418ffa 418 isHazColour[i] = true;
dreamselec 26:bbcc25418ffa 419 }
dreamselec 26:bbcc25418ffa 420 }
dreamselec 26:bbcc25418ffa 421
dreamselec 23:db91aaa43a9e 422 // for (int i = 0; i < 3; i++) {
dreamselec 23:db91aaa43a9e 423 // if (colourValues[i] < HazBlock->maxColour.components[i] && colourValues[i] > HazBlock->minColour.components[i]) {
dreamselec 23:db91aaa43a9e 424 // isHazColour[i] = true;
dreamselec 23:db91aaa43a9e 425 // }
dreamselec 23:db91aaa43a9e 426 // }
dreamselec 15:777390eb5afd 427
dreamselec 23:db91aaa43a9e 428 bool isHazBlock = false;
dreamselec 15:777390eb5afd 429
dreamselec 23:db91aaa43a9e 430 if (isHazColour[0] && isHazColour[1] && isHazColour[2]) {
dreamselec 23:db91aaa43a9e 431 isHazBlock = true;
dreamselec 23:db91aaa43a9e 432 } else {
dreamselec 23:db91aaa43a9e 433 isHazBlock = false;
dreamselec 23:db91aaa43a9e 434 }
dreamselec 23:db91aaa43a9e 435
dreamselec 23:db91aaa43a9e 436 return isHazBlock;
dreamselec 3:843b830ee8bd 437 }
dreamselec 3:843b830ee8bd 438
dreamselec 6:98fe30430194 439 void printPCDetectedText()
dreamselec 4:4eebb4de22a7 440 {
dreamselec 23:db91aaa43a9e 441 lcd->cls();
dreamselec 23:db91aaa43a9e 442 LCDFL();
dreamselec 23:db91aaa43a9e 443 lcd->printf("Detected PC.");
dreamselec 23:db91aaa43a9e 444 LCDSL();
dreamselec 23:db91aaa43a9e 445 lcd->printf("Connecting");
dreamselec 23:db91aaa43a9e 446 initPort();
dreamselec 10:16ba52f8e025 447 }
dreamselec 10:16ba52f8e025 448
dreamselec 10:16ba52f8e025 449 bool displayAbortDialog()
dreamselec 10:16ba52f8e025 450 {
dreamselec 23:db91aaa43a9e 451 while (i2cport->read_bit(11) == 1) {}
dreamselec 23:db91aaa43a9e 452 i2cport->write_bit(1, 12);
dreamselec 10:16ba52f8e025 453
dreamselec 23:db91aaa43a9e 454 lcd->cls();
dreamselec 23:db91aaa43a9e 455 LCDFL();
dreamselec 23:db91aaa43a9e 456 lcd->printf("Abort?");
dreamselec 23:db91aaa43a9e 457 LCDSL();
dreamselec 23:db91aaa43a9e 458 lcd->printf("1:Yes, 2,3,4:No");
dreamselec 23:db91aaa43a9e 459 int reply = 0;
dreamselec 23:db91aaa43a9e 460 do {
dreamselec 23:db91aaa43a9e 461 reply = readSwitches();
dreamselec 23:db91aaa43a9e 462 } while(reply == 0);
dreamselec 10:16ba52f8e025 463
dreamselec 23:db91aaa43a9e 464 D_LEDS_OFF();
dreamselec 23:db91aaa43a9e 465 if (reply == 1) {
dreamselec 23:db91aaa43a9e 466 // while (i2cport->read_bit(8)) { }
dreamselec 23:db91aaa43a9e 467 return true;
dreamselec 23:db91aaa43a9e 468 } else {
dreamselec 23:db91aaa43a9e 469 // while (i2cport->read_bit(9) || i2cport->read_bit(10) || i2cport->read_bit(11)) { }
dreamselec 23:db91aaa43a9e 470 return false;
dreamselec 23:db91aaa43a9e 471 }
dreamselec 3:843b830ee8bd 472 }
dreamselec 3:843b830ee8bd 473
dreamselec 15:777390eb5afd 474 void printServoInfoOnLCD()
dreamselec 15:777390eb5afd 475 {
dreamselec 23:db91aaa43a9e 476 lcd->cls();
dreamselec 23:db91aaa43a9e 477 lcd->locate(0,0);
dreamselec 23:db91aaa43a9e 478 if (fpga->stoppingServoPosition == Stop)
dreamselec 25:792540d69c49 479 lcd->printf("1:Top: Go");
dreamselec 23:db91aaa43a9e 480 else if (fpga->stoppingServoPosition == Go)
dreamselec 25:792540d69c49 481 lcd->printf("1:Top: Stop");
dreamselec 23:db91aaa43a9e 482
dreamselec 23:db91aaa43a9e 483 lcd->locate(1,0);
dreamselec 23:db91aaa43a9e 484 if (fpga->sortingServoPosition == NonHaz)
dreamselec 25:792540d69c49 485 lcd->printf("2:Bottom: Haz");
dreamselec 23:db91aaa43a9e 486 else if (fpga->sortingServoPosition == Haz)
dreamselec 25:792540d69c49 487 lcd->printf("2:Bottom: NonHaz");
dreamselec 23:db91aaa43a9e 488 }
dreamselec 23:db91aaa43a9e 489
dreamselec 23:db91aaa43a9e 490 void printServoInfoOnPC() {
dreamselec 23:db91aaa43a9e 491 if (fpga->stoppingServoPosition == Stop)
dreamselec 23:db91aaa43a9e 492 pc.printf(":<servos>1=Stop;");
dreamselec 23:db91aaa43a9e 493 else if (fpga->stoppingServoPosition == Go)
dreamselec 23:db91aaa43a9e 494 pc.printf(":<servos>1=Go;");
dreamselec 23:db91aaa43a9e 495
dreamselec 23:db91aaa43a9e 496 if (fpga->sortingServoPosition == NonHaz)
dreamselec 23:db91aaa43a9e 497 pc.printf(":<servos>2=NonHaz;");
dreamselec 23:db91aaa43a9e 498 else if (fpga->sortingServoPosition == Haz)
dreamselec 23:db91aaa43a9e 499 pc.printf(":<servos>2=Haz;");
dreamselec 10:16ba52f8e025 500 }
dreamselec 10:16ba52f8e025 501
dreamselec 15:777390eb5afd 502 void runInServoTestMode()
dreamselec 15:777390eb5afd 503 {
dreamselec 23:db91aaa43a9e 504 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 23:db91aaa43a9e 505 printServoInfoOnPC();
dreamselec 23:db91aaa43a9e 506 printServoInfoOnLCD();
dreamselec 23:db91aaa43a9e 507
dreamselec 23:db91aaa43a9e 508 i2cport->write_bit(1, 12);
dreamselec 23:db91aaa43a9e 509 i2cport->write_bit(1, 13);
dreamselec 23:db91aaa43a9e 510 i2cport->write_bit(1, 15);
dreamselec 23:db91aaa43a9e 511 int button = 0;
dreamselec 23:db91aaa43a9e 512 bool finished = false;
dreamselec 23:db91aaa43a9e 513 do {
dreamselec 23:db91aaa43a9e 514 button = readSwitches();
dreamselec 25:792540d69c49 515
dreamselec 25:792540d69c49 516 // gToggleServoNumber: 1 = Toggle top servo, 2 = Toggle bottom servo, 3 = Toggle both servos
dreamselec 26:bbcc25418ffa 517 if (gToggleServoNumber == 1){
dreamselec 25:792540d69c49 518 button = 1;
dreamselec 25:792540d69c49 519 gToggleServoNumber = 0;
dreamselec 25:792540d69c49 520 }else if (gToggleServoNumber == 2){
dreamselec 25:792540d69c49 521 button = 2;
dreamselec 26:bbcc25418ffa 522 gToggleServoNumber = 0;
dreamselec 25:792540d69c49 523 }
dreamselec 25:792540d69c49 524
dreamselec 23:db91aaa43a9e 525 if (button == 1) {
dreamselec 23:db91aaa43a9e 526 fpga->toggleStoppingServo();
dreamselec 23:db91aaa43a9e 527 printServoInfoOnLCD();
dreamselec 23:db91aaa43a9e 528 printServoInfoOnPC();
dreamselec 23:db91aaa43a9e 529 wait(kServoWait);
dreamselec 23:db91aaa43a9e 530 } else if (button == 2) {
dreamselec 23:db91aaa43a9e 531 fpga->toggleSortingServo();
dreamselec 23:db91aaa43a9e 532 printServoInfoOnLCD();
dreamselec 23:db91aaa43a9e 533 printServoInfoOnPC();
dreamselec 23:db91aaa43a9e 534 wait(kServoWait);
dreamselec 23:db91aaa43a9e 535 }
dreamselec 25:792540d69c49 536
dreamselec 23:db91aaa43a9e 537 finished = button == 4;
dreamselec 23:db91aaa43a9e 538 button = 0;
dreamselec 23:db91aaa43a9e 539 } while (finished == false && runServoTest == true);
dreamselec 23:db91aaa43a9e 540
dreamselec 23:db91aaa43a9e 541 D_LEDS_OFF();
dreamselec 23:db91aaa43a9e 542 lcd->cls();
dreamselec 23:db91aaa43a9e 543 lcd->locate(0,0);
dreamselec 23:db91aaa43a9e 544 lcd->printf("Done servo test");
dreamselec 23:db91aaa43a9e 545 if (runServoTest == true){
dreamselec 23:db91aaa43a9e 546 pc.printf(":<servos>test=pause;");
dreamselec 23:db91aaa43a9e 547 runServoTest = false;
dreamselec 23:db91aaa43a9e 548 }
dreamselec 23:db91aaa43a9e 549 wait(0.5);
dreamselec 23:db91aaa43a9e 550 lcd->cls();
dreamselec 23:db91aaa43a9e 551 return;
dreamselec 23:db91aaa43a9e 552 }
dreamselec 15:777390eb5afd 553
dreamselec 23:db91aaa43a9e 554 void printBeamInfoOnLCD(){
dreamselec 23:db91aaa43a9e 555 lcd->cls();
dreamselec 23:db91aaa43a9e 556 lcd->locate(0,0);
dreamselec 23:db91aaa43a9e 557 lcd->printf("Top:L1 Btm: L2");
dreamselec 23:db91aaa43a9e 558 lcd->locate(1,0);
dreamselec 23:db91aaa43a9e 559 lcd->printf("On:High, Off:Low");
dreamselec 23:db91aaa43a9e 560 }
dreamselec 23:db91aaa43a9e 561
dreamselec 23:db91aaa43a9e 562 void printBeamInfoOnPC(int topBeam, int bottomBeam){
dreamselec 25:792540d69c49 563 pc.printf(":<break_beam>2=%i,1=%i;", topBeam, bottomBeam);
dreamselec 23:db91aaa43a9e 564 }
dreamselec 23:db91aaa43a9e 565
dreamselec 23:db91aaa43a9e 566 void runInBreakBeamTestMode(){
dreamselec 23:db91aaa43a9e 567 turnOffTopLEDs();
dreamselec 23:db91aaa43a9e 568 i2cport->write_bit(1, 15);
dreamselec 23:db91aaa43a9e 569
dreamselec 23:db91aaa43a9e 570 int topBeamValue = fpga->getBeamValue(1);
dreamselec 23:db91aaa43a9e 571 int bottomBeamValue = fpga->getBeamValue(2);
dreamselec 23:db91aaa43a9e 572 printBeamInfoOnPC(topBeamValue, bottomBeamValue);
dreamselec 23:db91aaa43a9e 573 printBeamInfoOnLCD();
dreamselec 18:44a1c1a30166 574
dreamselec 23:db91aaa43a9e 575 int button = 0;
dreamselec 23:db91aaa43a9e 576 bool finished = false;
dreamselec 23:db91aaa43a9e 577 do{
dreamselec 23:db91aaa43a9e 578 button = readSwitches();
dreamselec 23:db91aaa43a9e 579 int currentTopBeamValue = fpga->getBeamValue(1);
dreamselec 23:db91aaa43a9e 580 int currentBottomBeamValue = fpga->getBeamValue(2);
dreamselec 24:02c61793f90b 581
dreamselec 24:02c61793f90b 582 // For debugging, hold down both 1 and 3 or 2 and 3
dreamselec 24:02c61793f90b 583 if (i2cport->read_bit(10) == 1){
dreamselec 24:02c61793f90b 584 currentTopBeamValue = i2cport->read_bit(8) && i2cport->read_bit(10);
dreamselec 24:02c61793f90b 585 currentBottomBeamValue = i2cport->read_bit(9) && i2cport->read_bit(10);
dreamselec 24:02c61793f90b 586 }
dreamselec 23:db91aaa43a9e 587 myLED1 = currentTopBeamValue;
dreamselec 23:db91aaa43a9e 588 myLED2 = currentBottomBeamValue;
dreamselec 23:db91aaa43a9e 589
dreamselec 23:db91aaa43a9e 590 if (currentTopBeamValue != topBeamValue || currentBottomBeamValue != bottomBeamValue ){
dreamselec 23:db91aaa43a9e 591 topBeamValue = currentTopBeamValue;
dreamselec 23:db91aaa43a9e 592 bottomBeamValue = currentBottomBeamValue;
dreamselec 23:db91aaa43a9e 593 printBeamInfoOnPC(topBeamValue, bottomBeamValue);
dreamselec 23:db91aaa43a9e 594 }
dreamselec 23:db91aaa43a9e 595 finished = button == 4;
dreamselec 23:db91aaa43a9e 596 }while (runBreakBeamTest == true && finished == false);
dreamselec 23:db91aaa43a9e 597
dreamselec 23:db91aaa43a9e 598 turnOffBottomLEDs();
dreamselec 23:db91aaa43a9e 599 lcd->cls();
dreamselec 23:db91aaa43a9e 600 lcd->locate(0,0);
dreamselec 23:db91aaa43a9e 601 lcd->printf("Finished test");
dreamselec 23:db91aaa43a9e 602 if (runBreakBeamTest == true){
dreamselec 23:db91aaa43a9e 603 pc.printf(":<break_beam>test=pause;");
dreamselec 23:db91aaa43a9e 604 runBreakBeamTest = false;
dreamselec 23:db91aaa43a9e 605 }
dreamselec 23:db91aaa43a9e 606 wait(0.5);
dreamselec 23:db91aaa43a9e 607 return;
dreamselec 2:7a55cb10259f 608 }
dreamselec 15:777390eb5afd 609
dreamselec 25:792540d69c49 610 void printColourSensorInfoOnLCD(int colourValues[]){
dreamselec 25:792540d69c49 611 float weightedValues[4];
dreamselec 25:792540d69c49 612
dreamselec 25:792540d69c49 613 for (int i = 0; i < 3; i++){
dreamselec 25:792540d69c49 614 weightedValues[i] = (float)colourValues[i] / (float)colourValues[3];
dreamselec 25:792540d69c49 615 }
dreamselec 26:bbcc25418ffa 616 //TODO: Print values on LCD
dreamselec 25:792540d69c49 617 lcd->cls();
dreamselec 25:792540d69c49 618 }
dreamselec 25:792540d69c49 619
dreamselec 25:792540d69c49 620 void printColourSensorInfoOnPC(int colourValues[]){
dreamselec 25:792540d69c49 621 pc.printf("!<colour_sensor>red=%i,green=%i,blue=%i,clear=%i;", colourValues[0], colourValues[1], colourValues[2], colourValues[3]);
dreamselec 25:792540d69c49 622 }
dreamselec 25:792540d69c49 623
dreamselec 25:792540d69c49 624 void runInColourSensorTestMode() {
dreamselec 25:792540d69c49 625 turnOffTopLEDs();
dreamselec 25:792540d69c49 626 i2cport->write_bit(1, 15);
dreamselec 25:792540d69c49 627 i2cport->write_bit(1, 12);
dreamselec 25:792540d69c49 628 lcd->cls();
dreamselec 25:792540d69c49 629 lcd->locate(0,0);
dreamselec 25:792540d69c49 630 lcd->printf("Colour Sensor");
dreamselec 25:792540d69c49 631 lcd->locate(1,0);
dreamselec 25:792540d69c49 632 lcd->printf("Test Mode");
dreamselec 25:792540d69c49 633 pc.printf("!<colour_sensor>i-time=%.3f", gIntegrationTime);
dreamselec 25:792540d69c49 634
dreamselec 25:792540d69c49 635 int button = 0;
dreamselec 25:792540d69c49 636 bool finished = false;
dreamselec 25:792540d69c49 637
dreamselec 25:792540d69c49 638 do {
dreamselec 25:792540d69c49 639 button = readSwitches();
dreamselec 25:792540d69c49 640
dreamselec 25:792540d69c49 641 if (getColourSensorValue == true){
dreamselec 25:792540d69c49 642 int colourValues[4];
dreamselec 25:792540d69c49 643 rgbSensor.getAllColors(colourValues);
dreamselec 25:792540d69c49 644 printColourSensorInfoOnPC(colourValues);
dreamselec 25:792540d69c49 645 printColourSensorInfoOnLCD(colourValues);
dreamselec 25:792540d69c49 646 getColourSensorValue = false;
dreamselec 25:792540d69c49 647 } else if (getBlockColourValue == true){
dreamselec 25:792540d69c49 648 int topBeam = fpga->getBeamValue(1);
dreamselec 25:792540d69c49 649
dreamselec 25:792540d69c49 650 while(topBeam == 0){
dreamselec 25:792540d69c49 651 topBeam = fpga->getBeamValue(1);
dreamselec 25:792540d69c49 652 }
dreamselec 25:792540d69c49 653 int colourValues[3][4];
dreamselec 25:792540d69c49 654 for (int i = 0; i < 3; i++) { memset(colourValues[i], 0, sizeof(colourValues)); }
dreamselec 25:792540d69c49 655 int valueCount = 0;
dreamselec 25:792540d69c49 656 do {
dreamselec 25:792540d69c49 657 rgbSensor.getAllColors(colourValues[valueCount]);
dreamselec 25:792540d69c49 658 } while (topBeam == 1 && valueCount < 3);
dreamselec 25:792540d69c49 659 int averageValues[4] = {0, 0, 0, 0};
dreamselec 25:792540d69c49 660 for (int i = 0; i < 3; i++) {
dreamselec 25:792540d69c49 661 averageValues[0] += colourValues[i][0];
dreamselec 25:792540d69c49 662 averageValues[1] += colourValues[i][1];
dreamselec 25:792540d69c49 663 averageValues[2] += colourValues[i][2];
dreamselec 25:792540d69c49 664 averageValues[3] += colourValues[i][3];
dreamselec 25:792540d69c49 665 }
dreamselec 25:792540d69c49 666 averageValues[0] = averageValues[0] / valueCount;
dreamselec 25:792540d69c49 667 averageValues[1] = averageValues[1] / valueCount;
dreamselec 25:792540d69c49 668 averageValues[2] = averageValues[2] / valueCount;
dreamselec 25:792540d69c49 669 averageValues[3] = averageValues[3] / valueCount;
dreamselec 25:792540d69c49 670
dreamselec 25:792540d69c49 671 printColourSensorInfoOnPC(averageValues);
dreamselec 25:792540d69c49 672 printColourSensorInfoOnLCD(averageValues);
dreamselec 25:792540d69c49 673
dreamselec 25:792540d69c49 674 getBlockColourValue = false;
dreamselec 25:792540d69c49 675 } else if (button == 1){
dreamselec 25:792540d69c49 676 getColourSensorValue = true;
dreamselec 25:792540d69c49 677 button = 0;
dreamselec 25:792540d69c49 678 }
dreamselec 25:792540d69c49 679
dreamselec 25:792540d69c49 680 finished = button == 4;
dreamselec 25:792540d69c49 681 // button = 0;
dreamselec 25:792540d69c49 682 } while (finished == false && runColourSensorTest == true);
dreamselec 25:792540d69c49 683
dreamselec 25:792540d69c49 684 turnOffBottomLEDs();
dreamselec 25:792540d69c49 685 lcd->cls();
dreamselec 25:792540d69c49 686 lcd->locate(0,0);
dreamselec 25:792540d69c49 687 lcd->printf("Finished test");
dreamselec 25:792540d69c49 688 if (runColourSensorTest == true){
dreamselec 25:792540d69c49 689 pc.printf(":<colour_sensor>test=pause;");
dreamselec 25:792540d69c49 690 runColourSensorTest = false;
dreamselec 25:792540d69c49 691 }
dreamselec 25:792540d69c49 692 wait(0.5);
dreamselec 25:792540d69c49 693 return;
dreamselec 25:792540d69c49 694 }
dreamselec 25:792540d69c49 695
dreamselec 15:777390eb5afd 696 void displayWaitingLine()
dreamselec 15:777390eb5afd 697 {
dreamselec 23:db91aaa43a9e 698 lcd->cls();
dreamselec 23:db91aaa43a9e 699 lcd->locate(0,0);
dreamselec 23:db91aaa43a9e 700 lcd->printf("Waiting...");
dreamselec 23:db91aaa43a9e 701 lcd->locate(1,0);
dreamselec 15:777390eb5afd 702 }
dreamselec 15:777390eb5afd 703
dreamselec 15:777390eb5afd 704 void displayPCStatus()
dreamselec 15:777390eb5afd 705 {
dreamselec 23:db91aaa43a9e 706 lcd->cls();
dreamselec 23:db91aaa43a9e 707 lcd->locate(0,0);
dreamselec 23:db91aaa43a9e 708 if (connectedToPC) {
dreamselec 23:db91aaa43a9e 709 if (currentMode == Normal)
dreamselec 23:db91aaa43a9e 710 lcd->printf("Normal mode.");
dreamselec 23:db91aaa43a9e 711 else if (currentMode == Maintanence)
dreamselec 23:db91aaa43a9e 712 lcd->printf("Maintanence.");
dreamselec 23:db91aaa43a9e 713 else if (currentMode == None)
dreamselec 23:db91aaa43a9e 714 lcd->printf("Connected to PC");
dreamselec 23:db91aaa43a9e 715 } else
dreamselec 23:db91aaa43a9e 716 lcd->printf("Waiting for PC..");
dreamselec 15:777390eb5afd 717
dreamselec 23:db91aaa43a9e 718 lcd->locate(1,0);
dreamselec 23:db91aaa43a9e 719 lcd->printf("4:Disconnect");
dreamselec 23:db91aaa43a9e 720 D_LEDS_OFF();
dreamselec 23:db91aaa43a9e 721 i2cport->write_bit(1,15);
dreamselec 18:44a1c1a30166 722 }
dreamselec 23:db91aaa43a9e 723
dreamselec 23:db91aaa43a9e 724 void turnOffTopLEDs(){
dreamselec 23:db91aaa43a9e 725 myLED1 = 0;
dreamselec 23:db91aaa43a9e 726 myLED2 = 0;
dreamselec 23:db91aaa43a9e 727 myLED3 = 0;
dreamselec 23:db91aaa43a9e 728 myLED4 = 0;
dreamselec 23:db91aaa43a9e 729 }
dreamselec 23:db91aaa43a9e 730
dreamselec 23:db91aaa43a9e 731 void turnOffBottomLEDs(){
dreamselec 23:db91aaa43a9e 732 i2cport->write_bit(0, 12);
dreamselec 23:db91aaa43a9e 733 i2cport->write_bit(0, 13);
dreamselec 23:db91aaa43a9e 734 i2cport->write_bit(0, 14);
dreamselec 23:db91aaa43a9e 735 i2cport->write_bit(0, 15);
dreamselec 23:db91aaa43a9e 736 }