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@22:993821a4c396, 2015-11-26 (annotated)

Committer:
dreamselec
Date:
Thu Nov 26 15:47:17 2015 +0000
Revision:
22:993821a4c396
Parent:
21:60c4fe94d79c
Child:
23:db91aaa43a9e
Taking 3 colour values and averaging it to determine the colour of the block.

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 20:4e0f0944f28f 48 //bool displayAbortDialog();
dreamselec 6:98fe30430194 49 void Rx_interrupt();
dreamselec 18:44a1c1a30166 50 bool checkColour(int colourValues[]);
dreamselec 10:16ba52f8e025 51 void runInServoTestMode();
dreamselec 15:777390eb5afd 52 void displayWaitingLine();
dreamselec 15:777390eb5afd 53 void displayPCStatus();
dreamselec 18:44a1c1a30166 54 bool waitForBlock();
dreamselec 18:44a1c1a30166 55 void sortBlock();
dreamselec 6:98fe30430194 56
dreamselec 6:98fe30430194 57 int main()
dreamselec 6:98fe30430194 58 {
dreamselec 19:61b21ac4896e 59 initInternal();
dreamselec 19:61b21ac4896e 60 initPort();
dreamselec 19:61b21ac4896e 61 srand((unsigned)time(NULL));
dreamselec 19:61b21ac4896e 62 U_LEDS_OFF();
dreamselec 19:61b21ac4896e 63 lcd->cls();
dreamselec 15:777390eb5afd 64
dreamselec 19:61b21ac4896e 65 rgbSensor.enablePowerAndRGBC();
dreamselec 20:4e0f0944f28f 66 rgbSensor.setIntegrationTime(3);
dreamselec 6:98fe30430194 67
dreamselec 19:61b21ac4896e 68 // Create a serial intereput for RxIrq so when PC is connected it sends '$' to tell MBED it's there.
dreamselec 19:61b21ac4896e 69 // https://developer.mbed.org/cookbook/Serial-Interrupts
dreamselec 19:61b21ac4896e 70 pc.attach(&Rx_interrupt, Serial::RxIrq);
dreamselec 6:98fe30430194 71
dreamselec 19:61b21ac4896e 72 for (;;) {
dreamselec 19:61b21ac4896e 73 lcd->cls();
dreamselec 19:61b21ac4896e 74 i2cport->write_bit(1, 12);
dreamselec 19:61b21ac4896e 75 lcd->printf("1: Start sorting.");
dreamselec 19:61b21ac4896e 76 LCDSL();
dreamselec 19:61b21ac4896e 77 i2cport->write_bit(1,13);
dreamselec 19:61b21ac4896e 78 lcd->printf("2: Connect to PC");
dreamselec 15:777390eb5afd 79
dreamselec 19:61b21ac4896e 80 fpga->moveStoppingServo(Stop);
dreamselec 19:61b21ac4896e 81 fpga->moveSortingServo(NonHaz);
dreamselec 10:16ba52f8e025 82
dreamselec 19:61b21ac4896e 83 int selection = 0;
dreamselec 19:61b21ac4896e 84 do {
dreamselec 19:61b21ac4896e 85 myLED4 = selection;
dreamselec 19:61b21ac4896e 86 selection = readSwitches();
dreamselec 19:61b21ac4896e 87 } while (selection != 1 && selection != 2 && connectedToPC == false);
dreamselec 19:61b21ac4896e 88 D_LEDS_OFF();
dreamselec 19:61b21ac4896e 89 if (selection == 1) {
dreamselec 19:61b21ac4896e 90 // User selected op 1: Start sorting autonomously.
dreamselec 19:61b21ac4896e 91 i2cport->write_bit(1, 12);
dreamselec 19:61b21ac4896e 92 lcd->cls();
dreamselec 19:61b21ac4896e 93 LCDFL();
dreamselec 19:61b21ac4896e 94 lcd->printf("Starting sorting");
dreamselec 19:61b21ac4896e 95 wait(0.5);
dreamselec 19:61b21ac4896e 96 D_LEDS_OFF();
dreamselec 19:61b21ac4896e 97
dreamselec 19:61b21ac4896e 98 for(;;) {
dreamselec 19:61b21ac4896e 99 displayWaitingLine();
dreamselec 19:61b21ac4896e 100 lcd->printf("for block.");
dreamselec 10:16ba52f8e025 101
dreamselec 19:61b21ac4896e 102 i2cport->write_bit(1, 15);
dreamselec 10:16ba52f8e025 103
dreamselec 19:61b21ac4896e 104 // Break and return to main menu i.e. Start Op, or Connect to PC.
dreamselec 19:61b21ac4896e 105 if (waitForBlock() == false) {
dreamselec 19:61b21ac4896e 106 D_LEDS_OFF();
dreamselec 19:61b21ac4896e 107 break;
dreamselec 19:61b21ac4896e 108 } else {
dreamselec 19:61b21ac4896e 109 sortBlock();
dreamselec 19:61b21ac4896e 110 }
dreamselec 19:61b21ac4896e 111 }
dreamselec 19:61b21ac4896e 112 }
dreamselec 10:16ba52f8e025 113
dreamselec 19:61b21ac4896e 114 if (selection == 2 || connectedToPC == true) {
dreamselec 19:61b21ac4896e 115 for (;;) {
dreamselec 19:61b21ac4896e 116 displayPCStatus();
dreamselec 19:61b21ac4896e 117
dreamselec 19:61b21ac4896e 118 i2cport->write_bit(1, 15);
dreamselec 19:61b21ac4896e 119 int abortOperation = false;
dreamselec 19:61b21ac4896e 120 while (connectedToPC == false && abortOperation == false) {
dreamselec 19:61b21ac4896e 121 abortOperation = readSwitches() == 4;
dreamselec 19:61b21ac4896e 122 }
dreamselec 18:44a1c1a30166 123
dreamselec 19:61b21ac4896e 124 if (abortOperation == true) {
dreamselec 19:61b21ac4896e 125 D_LEDS_OFF();
dreamselec 19:61b21ac4896e 126 break;
dreamselec 19:61b21ac4896e 127 }
dreamselec 18:44a1c1a30166 128
dreamselec 19:61b21ac4896e 129 displayPCStatus();
dreamselec 15:777390eb5afd 130
dreamselec 19:61b21ac4896e 131 while (abortOperation == false && connectedToPC == true) {
dreamselec 19:61b21ac4896e 132 if (currentMode == Maintanence) {
dreamselec 19:61b21ac4896e 133 if (runServoTest == true)
dreamselec 19:61b21ac4896e 134 runInServoTestMode();
dreamselec 19:61b21ac4896e 135 } else if (currentMode == Normal) {
dreamselec 19:61b21ac4896e 136 displayPCStatus();
dreamselec 19:61b21ac4896e 137 while (currentMode == Normal) {
dreamselec 19:61b21ac4896e 138 if (currentState == Pause) {
dreamselec 19:61b21ac4896e 139 lcd->cls();
dreamselec 19:61b21ac4896e 140 lcd->locate(0,0);
dreamselec 19:61b21ac4896e 141 lcd->printf("Sorting Paused.");
dreamselec 19:61b21ac4896e 142 lcd->locate(1,0);
dreamselec 19:61b21ac4896e 143 lcd->printf("1: Start");
dreamselec 19:61b21ac4896e 144 int button = 0;
dreamselec 20:4e0f0944f28f 145 i2cport->write_bit(1, 12);
dreamselec 20:4e0f0944f28f 146 i2cport->write_bit(1, 15);
dreamselec 20:4e0f0944f28f 147 while (currentState == Pause && currentMode == Normal) {
dreamselec 19:61b21ac4896e 148 button = readSwitches();
dreamselec 19:61b21ac4896e 149 if (button == 1) {
dreamselec 19:61b21ac4896e 150 pc.printf("state:start\n");
dreamselec 19:61b21ac4896e 151 currentState = Start;
dreamselec 19:61b21ac4896e 152 } else if (button == 4) {
dreamselec 20:4e0f0944f28f 153 currentMode = None;
dreamselec 20:4e0f0944f28f 154 // goto setModeNone;
dreamselec 20:4e0f0944f28f 155 break;
dreamselec 19:61b21ac4896e 156 }
dreamselec 19:61b21ac4896e 157 }
dreamselec 19:61b21ac4896e 158 }
dreamselec 19:61b21ac4896e 159 if (currentState == Start) {
dreamselec 20:4e0f0944f28f 160 lcd->cls();
dreamselec 20:4e0f0944f28f 161 lcd->locate(0,0);
dreamselec 20:4e0f0944f28f 162 lcd->printf("Sorting mode...");
dreamselec 20:4e0f0944f28f 163 while (currentState == Start && currentMode == Normal) {
dreamselec 19:61b21ac4896e 164 if (waitForBlock() == false) {
dreamselec 19:61b21ac4896e 165 if (connectedToPC == true) {
dreamselec 19:61b21ac4896e 166 // TODO: Tell PC to update UI if aborted from MBED.
dreamselec 19:61b21ac4896e 167 pc.printf("state:pause\n");
dreamselec 20:4e0f0944f28f 168 currentState = Pause;
dreamselec 19:61b21ac4896e 169 continue;
dreamselec 19:61b21ac4896e 170 } else if (connectedToPC == false) {
dreamselec 19:61b21ac4896e 171 currentMode = None;
dreamselec 19:61b21ac4896e 172 abortOperation = true;
dreamselec 19:61b21ac4896e 173 }
dreamselec 19:61b21ac4896e 174 } else {
dreamselec 19:61b21ac4896e 175 sortBlock();
dreamselec 19:61b21ac4896e 176 }
dreamselec 19:61b21ac4896e 177 }
dreamselec 19:61b21ac4896e 178 }
dreamselec 19:61b21ac4896e 179 }
dreamselec 19:61b21ac4896e 180 } else if (currentMode == None) {
dreamselec 19:61b21ac4896e 181 setModeNone:
dreamselec 20:4e0f0944f28f 182 D_LEDS_OFF();
dreamselec 20:4e0f0944f28f 183 i2cport->write_bit(1,15);
dreamselec 19:61b21ac4896e 184 displayPCStatus();
dreamselec 20:4e0f0944f28f 185 while (currentMode == None && abortOperation == false && connectedToPC == true) {
dreamselec 20:4e0f0944f28f 186 if (i2cport->read_bit(11)) {
dreamselec 20:4e0f0944f28f 187 abortOperation = displayAbortDialog();
dreamselec 20:4e0f0944f28f 188 // Cancel the Abort
dreamselec 20:4e0f0944f28f 189 if (abortOperation == false) {
dreamselec 20:4e0f0944f28f 190 displayPCStatus();
dreamselec 20:4e0f0944f28f 191 i2cport->write_bit(1, 15);
dreamselec 20:4e0f0944f28f 192 }
dreamselec 20:4e0f0944f28f 193 }
dreamselec 19:61b21ac4896e 194 }
dreamselec 19:61b21ac4896e 195 }
dreamselec 19:61b21ac4896e 196 }
dreamselec 15:777390eb5afd 197
dreamselec 19:61b21ac4896e 198 if (abortOperation == true ) {
dreamselec 19:61b21ac4896e 199 connectedToPC = false;
dreamselec 19:61b21ac4896e 200 D_LEDS_OFF();
dreamselec 19:61b21ac4896e 201 break;
dreamselec 19:61b21ac4896e 202 }
dreamselec 19:61b21ac4896e 203 }
dreamselec 19:61b21ac4896e 204 }
dreamselec 19:61b21ac4896e 205
dreamselec 19:61b21ac4896e 206 }
dreamselec 18:44a1c1a30166 207 }
dreamselec 8:e1da2ae62885 208
dreamselec 18:44a1c1a30166 209 // Waits until detects block.
dreamselec 18:44a1c1a30166 210 // true if block detected, false if cancelled/connected to PC.
dreamselec 19:61b21ac4896e 211 bool waitForBlock()
dreamselec 19:61b21ac4896e 212 {
dreamselec 20:4e0f0944f28f 213 myLED4 = 0;
dreamselec 20:4e0f0944f28f 214 myLED1 = 1;
dreamselec 19:61b21ac4896e 215 if (connectedToPC == false) {
dreamselec 19:61b21ac4896e 216 bool abortOperation = false;
dreamselec 19:61b21ac4896e 217 int blockInserted = 0;
dreamselec 19:61b21ac4896e 218 // Wait until a block is breaking the beam, or button 4 is pressed to abort.
dreamselec 19:61b21ac4896e 219 do {
dreamselec 19:61b21ac4896e 220 blockInserted = fpga->getBeamValue(Top);
dreamselec 19:61b21ac4896e 221 myLED4 = blockInserted;
dreamselec 19:61b21ac4896e 222 if (i2cport->read_bit(11)) {
dreamselec 19:61b21ac4896e 223 abortOperation = displayAbortDialog();
dreamselec 19:61b21ac4896e 224 // Cancel the Abort
dreamselec 19:61b21ac4896e 225 if (abortOperation == false) {
dreamselec 19:61b21ac4896e 226 displayWaitingLine();
dreamselec 19:61b21ac4896e 227 lcd->printf("for block");
dreamselec 19:61b21ac4896e 228 i2cport->write_bit(1, 15);
dreamselec 19:61b21ac4896e 229 }
dreamselec 19:61b21ac4896e 230 }
dreamselec 19:61b21ac4896e 231 } while (abortOperation == false && blockInserted != 1 && connectedToPC == false);
dreamselec 18:44a1c1a30166 232
dreamselec 19:61b21ac4896e 233 if (abortOperation == true || connectedToPC == true)
dreamselec 19:61b21ac4896e 234 return false;
dreamselec 19:61b21ac4896e 235 else
dreamselec 19:61b21ac4896e 236 return true;
dreamselec 19:61b21ac4896e 237 } else if (connectedToPC == true) {
dreamselec 19:61b21ac4896e 238 bool abortOperation = false;
dreamselec 19:61b21ac4896e 239 int blockInserted = 0;
dreamselec 19:61b21ac4896e 240 // Wait until a block is breaking the beam, or button 4 is pressed to abort.
dreamselec 19:61b21ac4896e 241 do {
dreamselec 19:61b21ac4896e 242 blockInserted = fpga->getBeamValue(Top);
dreamselec 19:61b21ac4896e 243 myLED4 = blockInserted;
dreamselec 19:61b21ac4896e 244 if (i2cport->read_bit(11)) {
dreamselec 19:61b21ac4896e 245 abortOperation = displayAbortDialog();
dreamselec 19:61b21ac4896e 246 // Cancel the Abort
dreamselec 19:61b21ac4896e 247 if (abortOperation == false) {
dreamselec 19:61b21ac4896e 248 displayWaitingLine();
dreamselec 19:61b21ac4896e 249 lcd->printf("for block");
dreamselec 19:61b21ac4896e 250 i2cport->write_bit(1, 15);
dreamselec 19:61b21ac4896e 251 }
dreamselec 19:61b21ac4896e 252 }
dreamselec 20:4e0f0944f28f 253 } while (abortOperation == false && blockInserted != 1 && connectedToPC == true && currentState == Start && currentMode == Normal);
dreamselec 19:61b21ac4896e 254
dreamselec 20:4e0f0944f28f 255 if (abortOperation == true || connectedToPC == false || currentState == Pause || currentMode == Maintanence)
dreamselec 19:61b21ac4896e 256 return false;
dreamselec 19:61b21ac4896e 257 else
dreamselec 19:61b21ac4896e 258 return true;
dreamselec 19:61b21ac4896e 259 }
dreamselec 18:44a1c1a30166 260 }
dreamselec 14:cf2f255b5560 261
dreamselec 19:61b21ac4896e 262 void sortBlock()
dreamselec 19:61b21ac4896e 263 {
dreamselec 20:4e0f0944f28f 264 myLED1 = 0;
dreamselec 20:4e0f0944f28f 265 myLED2 = 1;
dreamselec 19:61b21ac4896e 266 // Cannot Abort any longer. Block is inserted.
dreamselec 19:61b21ac4896e 267 // Detach rx interrupt until block processed.
dreamselec 19:61b21ac4896e 268 NVIC_DisableIRQ(UART1_IRQn);
dreamselec 21:60c4fe94d79c 269 fpga->moveSortingServo(Haz);
dreamselec 21:60c4fe94d79c 270 fpga->moveStoppingServo(Go);
dreamselec 21:60c4fe94d79c 271
dreamselec 19:61b21ac4896e 272 int colourValues[4];
dreamselec 22:993821a4c396 273 int averageColourValues[4] = {0, 0, 0, 0};
dreamselec 22:993821a4c396 274 for (int i = 0; i < 3; i++){
dreamselec 22:993821a4c396 275 rgbSensor.getAllColors(colourValues);
dreamselec 22:993821a4c396 276 for (int j = 0; j < 4; j++){
dreamselec 22:993821a4c396 277 averageColourValues[j] += colourValues[j];
dreamselec 22:993821a4c396 278 }
dreamselec 22:993821a4c396 279 }
dreamselec 22:993821a4c396 280 for (int i = 0; i < 4; i++){
dreamselec 22:993821a4c396 281 averageColourValues[i] = averageColourValues[i] / 3;
dreamselec 22:993821a4c396 282 }
dreamselec 18:44a1c1a30166 283
dreamselec 21:60c4fe94d79c 284 bool haz = false;
dreamselec 21:60c4fe94d79c 285 haz = checkColour(colourValues);
dreamselec 22:993821a4c396 286
dreamselec 21:60c4fe94d79c 287 if (!haz) {
dreamselec 21:60c4fe94d79c 288 fpga->moveSortingServo(NonHaz);
dreamselec 19:61b21ac4896e 289 }
dreamselec 22:993821a4c396 290 int blockSize;
dreamselec 22:993821a4c396 291 while (fpga->checkForBlock() == 0) { }
dreamselec 22:993821a4c396 292 blockSize = fpga->checkForSize();
dreamselec 22:993821a4c396 293 if (blockSize == HazBlock->size && haz) {
dreamselec 21:60c4fe94d79c 294 // fpga->moveSortingServo(Haz);
dreamselec 21:60c4fe94d79c 295 // fpga->moveStoppingServo(Go);
dreamselec 22:993821a4c396 296 // blockSize = HazBlock->size;
dreamselec 21:60c4fe94d79c 297 while(fpga->getBeamValue(Bottom) == 1) {}
dreamselec 21:60c4fe94d79c 298 wait(kServoWait);
dreamselec 21:60c4fe94d79c 299 fpga->moveStoppingServo(Stop);
dreamselec 19:61b21ac4896e 300 } else {
dreamselec 21:60c4fe94d79c 301 haz = false;
dreamselec 19:61b21ac4896e 302 }
dreamselec 21:60c4fe94d79c 303 fpga->moveSortingServo(NonHaz);
dreamselec 21:60c4fe94d79c 304 while(fpga->checkForSize()) {}
dreamselec 20:4e0f0944f28f 305
dreamselec 21:60c4fe94d79c 306 if (connectedToPC) {
dreamselec 21:60c4fe94d79c 307 for (int i = 0; i < 3; i++) {
dreamselec 21:60c4fe94d79c 308 pc.printf("DEBUG:Percentage Error: %.5f.\n", percentageError[i]);
dreamselec 21:60c4fe94d79c 309 if ((percentageError[i] < 0 && std::abs(percentageError[i]) < kMinError[i] * 2) || percentageError[i] == 0 || (percentageError[i] > 0 && percentageError[i] < kMaxError[i] * 2))
dreamselec 21:60c4fe94d79c 310 pc.printf("DEBUG:%i.\n", i);
dreamselec 21:60c4fe94d79c 311 }
dreamselec 22:993821a4c396 312 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 22:993821a4c396 313 // 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 21:60c4fe94d79c 314 }
dreamselec 18:44a1c1a30166 315
dreamselec 19:61b21ac4896e 316 // Re-Attach rx interrupt
dreamselec 19:61b21ac4896e 317 NVIC_EnableIRQ(UART1_IRQn);
dreamselec 20:4e0f0944f28f 318 myLED3 = 0;
dreamselec 20:4e0f0944f28f 319 myLED4 = 1;
dreamselec 19:61b21ac4896e 320 return;
dreamselec 6:98fe30430194 321 }
dreamselec 6:98fe30430194 322
dreamselec 6:98fe30430194 323 /// Called every-time it receives an char from PC.
dreamselec 10:16ba52f8e025 324 void Rx_interrupt()
dreamselec 10:16ba52f8e025 325 {
dreamselec 19:61b21ac4896e 326 char interruptChar = pc.getc();
dreamselec 19:61b21ac4896e 327 // Uncomment to Echo to USB serial to watch data flow
dreamselec 19:61b21ac4896e 328 // pc.putc(interruptChar);
dreamselec 6:98fe30430194 329
dreamselec 19:61b21ac4896e 330 NVIC_DisableIRQ(UART1_IRQn);
dreamselec 8:e1da2ae62885 331
dreamselec 19:61b21ac4896e 332 if (interruptChar == CommandTypeValue[Query]) {
dreamselec 19:61b21ac4896e 333 commander->decodeCommand(Query);
dreamselec 19:61b21ac4896e 334 } else if (interruptChar == CommandTypeValue[Set]) {
dreamselec 19:61b21ac4896e 335 commander->decodeCommand(Set);
dreamselec 19:61b21ac4896e 336 } else if (interruptChar== CommandTypeValue[Reply]) {
dreamselec 19:61b21ac4896e 337 commander->decodeCommand(Reply);
dreamselec 19:61b21ac4896e 338 }
dreamselec 8:e1da2ae62885 339
dreamselec 19:61b21ac4896e 340 NVIC_EnableIRQ(UART1_IRQn);
dreamselec 6:98fe30430194 341 }
dreamselec 3:843b830ee8bd 342
dreamselec 4:4eebb4de22a7 343 void initInternal()
dreamselec 4:4eebb4de22a7 344 {
dreamselec 20:4e0f0944f28f 345 myLED1 = 1;
dreamselec 19:61b21ac4896e 346 i2cport = new MCP23017(p9, p10, 0x40);
dreamselec 19:61b21ac4896e 347 lcd = new WattBob_TextLCD(i2cport);
dreamselec 19:61b21ac4896e 348 BACKLIGHT_ON(i2cport);
dreamselec 19:61b21ac4896e 349 lcd->cls();
dreamselec 20:4e0f0944f28f 350 lcd->locate(0,0);
dreamselec 19:61b21ac4896e 351 lcd->printf("Initilizing...");
dreamselec 20:4e0f0944f28f 352 DefaultHazBlock();
dreamselec 19:61b21ac4896e 353 myLED2 = 1;
dreamselec 19:61b21ac4896e 354 return;
dreamselec 3:843b830ee8bd 355 }
dreamselec 3:843b830ee8bd 356
dreamselec 7:b6e31bfdb2af 357 void initPort(int baudRate)
dreamselec 4:4eebb4de22a7 358 {
dreamselec 19:61b21ac4896e 359 myLED3 = 1;
dreamselec 19:61b21ac4896e 360 pc.baud(baudRate);
dreamselec 19:61b21ac4896e 361 pc.format(8, SerialBase::None, gStopBits);
dreamselec 19:61b21ac4896e 362 myLED4 = 1;
dreamselec 19:61b21ac4896e 363 wait (0.1);
dreamselec 19:61b21ac4896e 364 return;
dreamselec 3:843b830ee8bd 365 }
dreamselec 3:843b830ee8bd 366
dreamselec 18:44a1c1a30166 367 bool checkColour(int colourValues[])
dreamselec 15:777390eb5afd 368 {
dreamselec 20:4e0f0944f28f 369 myLED2 = 0;
dreamselec 20:4e0f0944f28f 370 myLED3 = 1;
dreamselec 20:4e0f0944f28f 371 bool isHazColour[3] = {false, false, false };
dreamselec 21:60c4fe94d79c 372 memset(adjustedValues, 0, sizeof(adjustedValues));
dreamselec 21:60c4fe94d79c 373 memset(percentageError, 0, sizeof(percentageError));
dreamselec 15:777390eb5afd 374
dreamselec 20:4e0f0944f28f 375 for (int i = 0; i < 3; i++) {
dreamselec 20:4e0f0944f28f 376 adjustedValues[i] = (float)colourValues[i]/(float)colourValues[3];
dreamselec 20:4e0f0944f28f 377 percentageError[i] = (adjustedValues[i] - kAverageRedBlock[i]) / kAverageRedBlock[i];
dreamselec 20:4e0f0944f28f 378
dreamselec 21:60c4fe94d79c 379 if ((percentageError[i] < 0 && std::abs(percentageError[i]) < kMinError[i] * 2) || percentageError[i] == 0 || (percentageError[i] > 0 && percentageError[i] < kMaxError[i] * 2)) {
dreamselec 19:61b21ac4896e 380 isHazColour[i] = true;
dreamselec 19:61b21ac4896e 381 }
dreamselec 19:61b21ac4896e 382 }
dreamselec 15:777390eb5afd 383
dreamselec 20:4e0f0944f28f 384 // for (int i = 0; i < 3; i++) {
dreamselec 20:4e0f0944f28f 385 // if (colourValues[i] < HazBlock->maxColour.components[i] && colourValues[i] > HazBlock->minColour.components[i]) {
dreamselec 20:4e0f0944f28f 386 // isHazColour[i] = true;
dreamselec 20:4e0f0944f28f 387 // }
dreamselec 20:4e0f0944f28f 388 // }
dreamselec 15:777390eb5afd 389
dreamselec 19:61b21ac4896e 390 bool isHazBlock = false;
dreamselec 15:777390eb5afd 391
dreamselec 20:4e0f0944f28f 392 if (isHazColour[0] && isHazColour[1] && isHazColour[2]) {
dreamselec 20:4e0f0944f28f 393 isHazBlock = true;
dreamselec 19:61b21ac4896e 394 } else {
dreamselec 20:4e0f0944f28f 395 isHazBlock = false;
dreamselec 19:61b21ac4896e 396 }
dreamselec 22:993821a4c396 397 // pc.printf("DEBUG:haz: %i\n", isHazBlock);
dreamselec 20:4e0f0944f28f 398 return isHazBlock;
dreamselec 3:843b830ee8bd 399 }
dreamselec 3:843b830ee8bd 400
dreamselec 6:98fe30430194 401 void printPCDetectedText()
dreamselec 4:4eebb4de22a7 402 {
dreamselec 19:61b21ac4896e 403 lcd->cls();
dreamselec 19:61b21ac4896e 404 LCDFL();
dreamselec 19:61b21ac4896e 405 lcd->printf("Detected PC.");
dreamselec 19:61b21ac4896e 406 LCDSL();
dreamselec 19:61b21ac4896e 407 lcd->printf("Connecting");
dreamselec 19:61b21ac4896e 408 initPort();
dreamselec 10:16ba52f8e025 409 }
dreamselec 10:16ba52f8e025 410
dreamselec 10:16ba52f8e025 411 bool displayAbortDialog()
dreamselec 10:16ba52f8e025 412 {
dreamselec 19:61b21ac4896e 413 while (i2cport->read_bit(11) == 1) {}
dreamselec 19:61b21ac4896e 414 i2cport->write_bit(1, 12);
dreamselec 10:16ba52f8e025 415
dreamselec 19:61b21ac4896e 416 lcd->cls();
dreamselec 19:61b21ac4896e 417 LCDFL();
dreamselec 19:61b21ac4896e 418 lcd->printf("Abort?");
dreamselec 19:61b21ac4896e 419 LCDSL();
dreamselec 19:61b21ac4896e 420 lcd->printf("1:Yes, 2,3,4:No");
dreamselec 19:61b21ac4896e 421 int reply = 0;
dreamselec 19:61b21ac4896e 422 do {
dreamselec 19:61b21ac4896e 423 reply = readSwitches();
dreamselec 19:61b21ac4896e 424 } while(reply == 0);
dreamselec 10:16ba52f8e025 425
dreamselec 19:61b21ac4896e 426 D_LEDS_OFF();
dreamselec 19:61b21ac4896e 427 if (reply == 1) {
dreamselec 19:61b21ac4896e 428 // while (i2cport->read_bit(8)) { }
dreamselec 19:61b21ac4896e 429 return true;
dreamselec 19:61b21ac4896e 430 } else {
dreamselec 19:61b21ac4896e 431 // while (i2cport->read_bit(9) || i2cport->read_bit(10) || i2cport->read_bit(11)) { }
dreamselec 19:61b21ac4896e 432 return false;
dreamselec 19:61b21ac4896e 433 }
dreamselec 3:843b830ee8bd 434 }
dreamselec 3:843b830ee8bd 435
dreamselec 15:777390eb5afd 436 void printServoInfoOnLCD()
dreamselec 15:777390eb5afd 437 {
dreamselec 19:61b21ac4896e 438 lcd->cls();
dreamselec 19:61b21ac4896e 439 lcd->locate(0,0);
dreamselec 19:61b21ac4896e 440 lcd->printf("1: Stop servo:%i", fpga->stoppingServoPosition);
dreamselec 19:61b21ac4896e 441 lcd->locate(1,0);
dreamselec 19:61b21ac4896e 442 lcd->printf("2: Sort servo:%i", fpga->sortingServoPosition);
dreamselec 10:16ba52f8e025 443 }
dreamselec 10:16ba52f8e025 444
dreamselec 15:777390eb5afd 445 void runInServoTestMode()
dreamselec 15:777390eb5afd 446 {
dreamselec 19:61b21ac4896e 447 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 19:61b21ac4896e 448 printServoInfoOnLCD();
dreamselec 19:61b21ac4896e 449 int finished = false;
dreamselec 15:777390eb5afd 450
dreamselec 19:61b21ac4896e 451 i2cport->write_bit(1, 12);
dreamselec 19:61b21ac4896e 452 i2cport->write_bit(1, 13);
dreamselec 19:61b21ac4896e 453 i2cport->write_bit(1, 15);
dreamselec 19:61b21ac4896e 454 int button = 0;
dreamselec 19:61b21ac4896e 455 do {
dreamselec 19:61b21ac4896e 456 button = readSwitches();
dreamselec 19:61b21ac4896e 457 if (button == 1) {
dreamselec 19:61b21ac4896e 458 fpga->toggleStoppingServo();
dreamselec 19:61b21ac4896e 459 printServoInfoOnLCD();
dreamselec 20:4e0f0944f28f 460 wait(kServoWait);
dreamselec 19:61b21ac4896e 461 } else if (button == 2) {
dreamselec 19:61b21ac4896e 462 fpga->toggleSortingServo();
dreamselec 19:61b21ac4896e 463 printServoInfoOnLCD();
dreamselec 20:4e0f0944f28f 464 wait(kServoWait);
dreamselec 19:61b21ac4896e 465 }
dreamselec 19:61b21ac4896e 466 finished = button == 4;
dreamselec 19:61b21ac4896e 467 button = 0;
dreamselec 19:61b21ac4896e 468 } while (finished == false && runServoTest != false);
dreamselec 18:44a1c1a30166 469
dreamselec 19:61b21ac4896e 470 D_LEDS_OFF();
dreamselec 19:61b21ac4896e 471 lcd->cls();
dreamselec 19:61b21ac4896e 472 lcd->locate(0,0);
dreamselec 19:61b21ac4896e 473 lcd->printf("Done servo test");
dreamselec 19:61b21ac4896e 474 //TODO: Inform PC of end.
dreamselec 19:61b21ac4896e 475 wait(0.5);
dreamselec 19:61b21ac4896e 476 lcd->cls();
dreamselec 19:61b21ac4896e 477 return;
dreamselec 2:7a55cb10259f 478 }
dreamselec 15:777390eb5afd 479
dreamselec 15:777390eb5afd 480 void displayWaitingLine()
dreamselec 15:777390eb5afd 481 {
dreamselec 19:61b21ac4896e 482 lcd->cls();
dreamselec 19:61b21ac4896e 483 lcd->locate(0,0);
dreamselec 19:61b21ac4896e 484 lcd->printf("Waiting...");
dreamselec 19:61b21ac4896e 485 lcd->locate(1,0);
dreamselec 15:777390eb5afd 486 }
dreamselec 15:777390eb5afd 487
dreamselec 15:777390eb5afd 488 void displayPCStatus()
dreamselec 15:777390eb5afd 489 {
dreamselec 19:61b21ac4896e 490 lcd->cls();
dreamselec 19:61b21ac4896e 491 lcd->locate(0,0);
dreamselec 19:61b21ac4896e 492 if (connectedToPC) {
dreamselec 19:61b21ac4896e 493 if (currentMode == Normal)
dreamselec 19:61b21ac4896e 494 lcd->printf("Normal mode.");
dreamselec 19:61b21ac4896e 495 else if (currentMode == Maintanence)
dreamselec 19:61b21ac4896e 496 lcd->printf("Maintanence.");
dreamselec 19:61b21ac4896e 497 else if (currentMode == None)
dreamselec 19:61b21ac4896e 498 lcd->printf("Connected to PC");
dreamselec 19:61b21ac4896e 499 } else
dreamselec 19:61b21ac4896e 500 lcd->printf("Waiting for PC..");
dreamselec 15:777390eb5afd 501
dreamselec 19:61b21ac4896e 502 lcd->locate(1,0);
dreamselec 19:61b21ac4896e 503 lcd->printf("4:Disconnect");
dreamselec 19:61b21ac4896e 504 D_LEDS_OFF();
dreamselec 19:61b21ac4896e 505 i2cport->write_bit(1,15);
dreamselec 18:44a1c1a30166 506 }