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@25:792540d69c49, 2015-11-30 (annotated)

Committer:
dreamselec
Date:
Mon Nov 30 15:06:38 2015 +0000
Revision:
25:792540d69c49
Parent:
24:02c61793f90b
Child:
26:bbcc25418ffa
Colour sensor test mode, new commands for servo to toggle. Needs to be tested with hardware.

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