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@29:9c0339e3c593, 2015-12-01 (annotated)

Committer:
dreamselec
Date:
Tue Dec 01 22:56:08 2015 +0000
Revision:
29:9c0339e3c593
Parent:
28:7e4d29977d72
Child:
30:c0bc92d009fe
Set new haz block mode should work.

Added hardcoded other colour values.

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