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@21:60c4fe94d79c, 2015-11-25 (annotated)

Committer:
dreamselec
Date:
Wed Nov 25 19:21:12 2015 +0000
Revision:
21:60c4fe94d79c
Parent:
20:4e0f0944f28f
Child:
22:993821a4c396
Optimised the code so, bottom servo is fast enough to sort the blocks. Code starts moving the servo after it detects block, checks the colour, then the size, if size and colour don't match then it moves the servo back to non haz.

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 19:61b21ac4896e 273 rgbSensor.getAllColors(colourValues);
dreamselec 18:44a1c1a30166 274
dreamselec 21:60c4fe94d79c 275 bool haz = false;
dreamselec 21:60c4fe94d79c 276 haz = checkColour(colourValues);
dreamselec 18:44a1c1a30166 277
dreamselec 21:60c4fe94d79c 278 if (!haz) {
dreamselec 21:60c4fe94d79c 279 fpga->moveSortingServo(NonHaz);
dreamselec 19:61b21ac4896e 280 }
dreamselec 15:777390eb5afd 281
dreamselec 21:60c4fe94d79c 282 while (fpga->checkForBlock() == 0) { }
dreamselec 21:60c4fe94d79c 283 int blockSize;
dreamselec 21:60c4fe94d79c 284 if (fpga->checkForSize() == HazBlock->size && haz) {
dreamselec 21:60c4fe94d79c 285 // fpga->moveSortingServo(Haz);
dreamselec 21:60c4fe94d79c 286 // fpga->moveStoppingServo(Go);
dreamselec 21:60c4fe94d79c 287 int blockSize = HazBlock->size;
dreamselec 21:60c4fe94d79c 288 while(fpga->getBeamValue(Bottom) == 1) {}
dreamselec 21:60c4fe94d79c 289 wait(kServoWait);
dreamselec 21:60c4fe94d79c 290 fpga->moveStoppingServo(Stop);
dreamselec 19:61b21ac4896e 291 } else {
dreamselec 21:60c4fe94d79c 292 haz = false;
dreamselec 19:61b21ac4896e 293 }
dreamselec 21:60c4fe94d79c 294 fpga->moveSortingServo(NonHaz);
dreamselec 21:60c4fe94d79c 295 while(fpga->checkForSize()) {}
dreamselec 20:4e0f0944f28f 296
dreamselec 21:60c4fe94d79c 297 if (connectedToPC) {
dreamselec 21:60c4fe94d79c 298 for (int i = 0; i < 3; i++) {
dreamselec 21:60c4fe94d79c 299 pc.printf("DEBUG:Percentage Error: %.5f.\n", percentageError[i]);
dreamselec 21:60c4fe94d79c 300 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 301 pc.printf("DEBUG:%i.\n", i);
dreamselec 21:60c4fe94d79c 302 }
dreamselec 21:60c4fe94d79c 303 pc.printf("BLOCK:Red:%.3f,Green:%.3f,Blue:%.3f,Haz:%i;", adjustedValues[0], adjustedValues[1], adjustedValues[2], haz);
dreamselec 21:60c4fe94d79c 304 pc.printf("BLOCK:Size:%i,Red:%i,Green:%i,Blue:%i,Clear:%i;", blockSize, colourValues[0], colourValues[1], colourValues[2], colourValues[3], haz);
dreamselec 21:60c4fe94d79c 305 }
dreamselec 18:44a1c1a30166 306
dreamselec 19:61b21ac4896e 307 // Re-Attach rx interrupt
dreamselec 19:61b21ac4896e 308 NVIC_EnableIRQ(UART1_IRQn);
dreamselec 20:4e0f0944f28f 309 myLED3 = 0;
dreamselec 20:4e0f0944f28f 310 myLED4 = 1;
dreamselec 19:61b21ac4896e 311 return;
dreamselec 6:98fe30430194 312 }
dreamselec 6:98fe30430194 313
dreamselec 6:98fe30430194 314 /// Called every-time it receives an char from PC.
dreamselec 10:16ba52f8e025 315 void Rx_interrupt()
dreamselec 10:16ba52f8e025 316 {
dreamselec 19:61b21ac4896e 317 char interruptChar = pc.getc();
dreamselec 19:61b21ac4896e 318 // Uncomment to Echo to USB serial to watch data flow
dreamselec 19:61b21ac4896e 319 // pc.putc(interruptChar);
dreamselec 6:98fe30430194 320
dreamselec 19:61b21ac4896e 321 NVIC_DisableIRQ(UART1_IRQn);
dreamselec 8:e1da2ae62885 322
dreamselec 19:61b21ac4896e 323 if (interruptChar == CommandTypeValue[Query]) {
dreamselec 19:61b21ac4896e 324 commander->decodeCommand(Query);
dreamselec 19:61b21ac4896e 325 } else if (interruptChar == CommandTypeValue[Set]) {
dreamselec 19:61b21ac4896e 326 commander->decodeCommand(Set);
dreamselec 19:61b21ac4896e 327 } else if (interruptChar== CommandTypeValue[Reply]) {
dreamselec 19:61b21ac4896e 328 commander->decodeCommand(Reply);
dreamselec 19:61b21ac4896e 329 }
dreamselec 8:e1da2ae62885 330
dreamselec 19:61b21ac4896e 331 NVIC_EnableIRQ(UART1_IRQn);
dreamselec 6:98fe30430194 332 }
dreamselec 3:843b830ee8bd 333
dreamselec 4:4eebb4de22a7 334 void initInternal()
dreamselec 4:4eebb4de22a7 335 {
dreamselec 20:4e0f0944f28f 336 myLED1 = 1;
dreamselec 19:61b21ac4896e 337 i2cport = new MCP23017(p9, p10, 0x40);
dreamselec 19:61b21ac4896e 338 lcd = new WattBob_TextLCD(i2cport);
dreamselec 19:61b21ac4896e 339 BACKLIGHT_ON(i2cport);
dreamselec 19:61b21ac4896e 340 lcd->cls();
dreamselec 20:4e0f0944f28f 341 lcd->locate(0,0);
dreamselec 19:61b21ac4896e 342 lcd->printf("Initilizing...");
dreamselec 20:4e0f0944f28f 343 DefaultHazBlock();
dreamselec 19:61b21ac4896e 344 myLED2 = 1;
dreamselec 19:61b21ac4896e 345 return;
dreamselec 3:843b830ee8bd 346 }
dreamselec 3:843b830ee8bd 347
dreamselec 7:b6e31bfdb2af 348 void initPort(int baudRate)
dreamselec 4:4eebb4de22a7 349 {
dreamselec 19:61b21ac4896e 350 myLED3 = 1;
dreamselec 19:61b21ac4896e 351 pc.baud(baudRate);
dreamselec 19:61b21ac4896e 352 pc.format(8, SerialBase::None, gStopBits);
dreamselec 19:61b21ac4896e 353 myLED4 = 1;
dreamselec 19:61b21ac4896e 354 wait (0.1);
dreamselec 19:61b21ac4896e 355 return;
dreamselec 3:843b830ee8bd 356 }
dreamselec 3:843b830ee8bd 357
dreamselec 18:44a1c1a30166 358 bool checkColour(int colourValues[])
dreamselec 15:777390eb5afd 359 {
dreamselec 20:4e0f0944f28f 360 myLED2 = 0;
dreamselec 20:4e0f0944f28f 361 myLED3 = 1;
dreamselec 20:4e0f0944f28f 362 bool isHazColour[3] = {false, false, false };
dreamselec 21:60c4fe94d79c 363 memset(adjustedValues, 0, sizeof(adjustedValues));
dreamselec 21:60c4fe94d79c 364 memset(percentageError, 0, sizeof(percentageError));
dreamselec 15:777390eb5afd 365
dreamselec 20:4e0f0944f28f 366 for (int i = 0; i < 3; i++) {
dreamselec 20:4e0f0944f28f 367 adjustedValues[i] = (float)colourValues[i]/(float)colourValues[3];
dreamselec 20:4e0f0944f28f 368 percentageError[i] = (adjustedValues[i] - kAverageRedBlock[i]) / kAverageRedBlock[i];
dreamselec 20:4e0f0944f28f 369
dreamselec 21:60c4fe94d79c 370 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 371 isHazColour[i] = true;
dreamselec 19:61b21ac4896e 372 }
dreamselec 19:61b21ac4896e 373 }
dreamselec 15:777390eb5afd 374
dreamselec 20:4e0f0944f28f 375 // for (int i = 0; i < 3; i++) {
dreamselec 20:4e0f0944f28f 376 // if (colourValues[i] < HazBlock->maxColour.components[i] && colourValues[i] > HazBlock->minColour.components[i]) {
dreamselec 20:4e0f0944f28f 377 // isHazColour[i] = true;
dreamselec 20:4e0f0944f28f 378 // }
dreamselec 20:4e0f0944f28f 379 // }
dreamselec 15:777390eb5afd 380
dreamselec 19:61b21ac4896e 381 bool isHazBlock = false;
dreamselec 15:777390eb5afd 382
dreamselec 20:4e0f0944f28f 383 if (isHazColour[0] && isHazColour[1] && isHazColour[2]) {
dreamselec 20:4e0f0944f28f 384 isHazBlock = true;
dreamselec 19:61b21ac4896e 385 } else {
dreamselec 20:4e0f0944f28f 386 isHazBlock = false;
dreamselec 19:61b21ac4896e 387 }
dreamselec 20:4e0f0944f28f 388 return isHazBlock;
dreamselec 3:843b830ee8bd 389 }
dreamselec 3:843b830ee8bd 390
dreamselec 6:98fe30430194 391 void printPCDetectedText()
dreamselec 4:4eebb4de22a7 392 {
dreamselec 19:61b21ac4896e 393 lcd->cls();
dreamselec 19:61b21ac4896e 394 LCDFL();
dreamselec 19:61b21ac4896e 395 lcd->printf("Detected PC.");
dreamselec 19:61b21ac4896e 396 LCDSL();
dreamselec 19:61b21ac4896e 397 lcd->printf("Connecting");
dreamselec 19:61b21ac4896e 398 initPort();
dreamselec 10:16ba52f8e025 399 }
dreamselec 10:16ba52f8e025 400
dreamselec 10:16ba52f8e025 401 bool displayAbortDialog()
dreamselec 10:16ba52f8e025 402 {
dreamselec 19:61b21ac4896e 403 while (i2cport->read_bit(11) == 1) {}
dreamselec 19:61b21ac4896e 404 i2cport->write_bit(1, 12);
dreamselec 10:16ba52f8e025 405
dreamselec 19:61b21ac4896e 406 lcd->cls();
dreamselec 19:61b21ac4896e 407 LCDFL();
dreamselec 19:61b21ac4896e 408 lcd->printf("Abort?");
dreamselec 19:61b21ac4896e 409 LCDSL();
dreamselec 19:61b21ac4896e 410 lcd->printf("1:Yes, 2,3,4:No");
dreamselec 19:61b21ac4896e 411 int reply = 0;
dreamselec 19:61b21ac4896e 412 do {
dreamselec 19:61b21ac4896e 413 reply = readSwitches();
dreamselec 19:61b21ac4896e 414 } while(reply == 0);
dreamselec 10:16ba52f8e025 415
dreamselec 19:61b21ac4896e 416 D_LEDS_OFF();
dreamselec 19:61b21ac4896e 417 if (reply == 1) {
dreamselec 19:61b21ac4896e 418 // while (i2cport->read_bit(8)) { }
dreamselec 19:61b21ac4896e 419 return true;
dreamselec 19:61b21ac4896e 420 } else {
dreamselec 19:61b21ac4896e 421 // while (i2cport->read_bit(9) || i2cport->read_bit(10) || i2cport->read_bit(11)) { }
dreamselec 19:61b21ac4896e 422 return false;
dreamselec 19:61b21ac4896e 423 }
dreamselec 3:843b830ee8bd 424 }
dreamselec 3:843b830ee8bd 425
dreamselec 15:777390eb5afd 426 void printServoInfoOnLCD()
dreamselec 15:777390eb5afd 427 {
dreamselec 19:61b21ac4896e 428 lcd->cls();
dreamselec 19:61b21ac4896e 429 lcd->locate(0,0);
dreamselec 19:61b21ac4896e 430 lcd->printf("1: Stop servo:%i", fpga->stoppingServoPosition);
dreamselec 19:61b21ac4896e 431 lcd->locate(1,0);
dreamselec 19:61b21ac4896e 432 lcd->printf("2: Sort servo:%i", fpga->sortingServoPosition);
dreamselec 10:16ba52f8e025 433 }
dreamselec 10:16ba52f8e025 434
dreamselec 15:777390eb5afd 435 void runInServoTestMode()
dreamselec 15:777390eb5afd 436 {
dreamselec 19:61b21ac4896e 437 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 438 printServoInfoOnLCD();
dreamselec 19:61b21ac4896e 439 int finished = false;
dreamselec 15:777390eb5afd 440
dreamselec 19:61b21ac4896e 441 i2cport->write_bit(1, 12);
dreamselec 19:61b21ac4896e 442 i2cport->write_bit(1, 13);
dreamselec 19:61b21ac4896e 443 i2cport->write_bit(1, 15);
dreamselec 19:61b21ac4896e 444 int button = 0;
dreamselec 19:61b21ac4896e 445 do {
dreamselec 19:61b21ac4896e 446 button = readSwitches();
dreamselec 19:61b21ac4896e 447 if (button == 1) {
dreamselec 19:61b21ac4896e 448 fpga->toggleStoppingServo();
dreamselec 19:61b21ac4896e 449 printServoInfoOnLCD();
dreamselec 20:4e0f0944f28f 450 wait(kServoWait);
dreamselec 19:61b21ac4896e 451 } else if (button == 2) {
dreamselec 19:61b21ac4896e 452 fpga->toggleSortingServo();
dreamselec 19:61b21ac4896e 453 printServoInfoOnLCD();
dreamselec 20:4e0f0944f28f 454 wait(kServoWait);
dreamselec 19:61b21ac4896e 455 }
dreamselec 19:61b21ac4896e 456 finished = button == 4;
dreamselec 19:61b21ac4896e 457 button = 0;
dreamselec 19:61b21ac4896e 458 } while (finished == false && runServoTest != false);
dreamselec 18:44a1c1a30166 459
dreamselec 19:61b21ac4896e 460 D_LEDS_OFF();
dreamselec 19:61b21ac4896e 461 lcd->cls();
dreamselec 19:61b21ac4896e 462 lcd->locate(0,0);
dreamselec 19:61b21ac4896e 463 lcd->printf("Done servo test");
dreamselec 19:61b21ac4896e 464 //TODO: Inform PC of end.
dreamselec 19:61b21ac4896e 465 wait(0.5);
dreamselec 19:61b21ac4896e 466 lcd->cls();
dreamselec 19:61b21ac4896e 467 return;
dreamselec 2:7a55cb10259f 468 }
dreamselec 15:777390eb5afd 469
dreamselec 15:777390eb5afd 470 void displayWaitingLine()
dreamselec 15:777390eb5afd 471 {
dreamselec 19:61b21ac4896e 472 lcd->cls();
dreamselec 19:61b21ac4896e 473 lcd->locate(0,0);
dreamselec 19:61b21ac4896e 474 lcd->printf("Waiting...");
dreamselec 19:61b21ac4896e 475 lcd->locate(1,0);
dreamselec 15:777390eb5afd 476 }
dreamselec 15:777390eb5afd 477
dreamselec 15:777390eb5afd 478 void displayPCStatus()
dreamselec 15:777390eb5afd 479 {
dreamselec 19:61b21ac4896e 480 lcd->cls();
dreamselec 19:61b21ac4896e 481 lcd->locate(0,0);
dreamselec 19:61b21ac4896e 482 if (connectedToPC) {
dreamselec 19:61b21ac4896e 483 if (currentMode == Normal)
dreamselec 19:61b21ac4896e 484 lcd->printf("Normal mode.");
dreamselec 19:61b21ac4896e 485 else if (currentMode == Maintanence)
dreamselec 19:61b21ac4896e 486 lcd->printf("Maintanence.");
dreamselec 19:61b21ac4896e 487 else if (currentMode == None)
dreamselec 19:61b21ac4896e 488 lcd->printf("Connected to PC");
dreamselec 19:61b21ac4896e 489 } else
dreamselec 19:61b21ac4896e 490 lcd->printf("Waiting for PC..");
dreamselec 15:777390eb5afd 491
dreamselec 19:61b21ac4896e 492 lcd->locate(1,0);
dreamselec 19:61b21ac4896e 493 lcd->printf("4:Disconnect");
dreamselec 19:61b21ac4896e 494 D_LEDS_OFF();
dreamselec 19:61b21ac4896e 495 i2cport->write_bit(1,15);
dreamselec 18:44a1c1a30166 496 }