![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
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
Diff: main.cpp
- Revision:
- 26:bbcc25418ffa
- Parent:
- 25:792540d69c49
- Child:
- 27:2cb1bdb7ae3d
diff -r 792540d69c49 -r bbcc25418ffa main.cpp --- a/main.cpp Mon Nov 30 15:06:38 2015 +0000 +++ b/main.cpp Mon Nov 30 21:29:51 2015 +0000 @@ -72,18 +72,21 @@ // https://developer.mbed.org/cookbook/Serial-Interrupts pc.attach(&Rx_interrupt, Serial::RxIrq); + DefaultHazBlock(); + fpga->moveStoppingServo(Stop); + fpga->moveSortingServo(NonHaz); + pc.printf(":<pc>connect;"); + for (;;) { + if (connectedToPC == false){ lcd->cls(); i2cport->write_bit(1, 12); lcd->printf("1: Start sorting."); LCDSL(); i2cport->write_bit(1,13); lcd->printf("2: Connect to PC"); - - fpga->moveStoppingServo(Stop); - fpga->moveSortingServo(NonHaz); + } - pc.printf(":<pc>connect;"); // wait(0.03); int selection = 0; @@ -146,6 +149,14 @@ runInBreakBeamTestMode(); else if (runColourSensorTest == true) runInColourSensorTestMode(); + if (i2cport->read_bit(11) == 1){ + if (displayAbortDialog() == true){ + currentMode = None; + pc.printf(":<mbed>mode=none;"); + }else { + displayPCStatus(); + } + } } } else if (currentMode == Normal) { displayPCStatus(); @@ -162,10 +173,11 @@ while (currentState == Pause && currentMode == Normal) { button = readSwitches(); if (button == 1) { - pc.printf("state:start\n"); + pc.printf(":<mbed>sort=start;"); currentState = Start; } else if (button == 4) { currentMode = None; + pc.printf(":<mbed>mode=none;"); // goto setModeNone; break; } @@ -177,9 +189,9 @@ lcd->printf("Sorting mode..."); while (currentState == Start && currentMode == Normal) { if (waitForBlock() == false) { - if (connectedToPC == true) { + if (connectedToPC == true && currentState != Pause) { // TODO: Tell PC to update UI if aborted from MBED. - pc.printf("state:pause\n"); + pc.printf(":<mbed>sort=pause;"); currentState = Pause; continue; } else if (connectedToPC == false) { @@ -272,6 +284,7 @@ else return true; } + return false; } void sortBlock() @@ -322,7 +335,7 @@ for (int i = 0; i < 3; i++) { pc.printf("DEBUG:Percentage Error: %.5f.\n", percentageError[i]); if ((percentageError[i] < 0 && std::abs(percentageError[i]) < kMinError[i] * 2) || percentageError[i] == 0 || (percentageError[i] > 0 && percentageError[i] < kMaxError[i] * 2)) - pc.printf("DEBUG:%i.\n", i); + pc.printf("DEBUG:%i Pass.\n", i); } 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]); // 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); @@ -387,6 +400,7 @@ memset(adjustedValues, 0, sizeof(adjustedValues)); memset(percentageError, 0, sizeof(percentageError)); +/* Harcoded working for (int i = 0; i < 3; i++) { adjustedValues[i] = (float)colourValues[i]/(float)colourValues[3]; percentageError[i] = (adjustedValues[i] - kAverageRedBlock[i]) / kAverageRedBlock[i]; @@ -395,7 +409,16 @@ isHazColour[i] = true; } } +*/ + for (int i = 0; i < 3; i++) { + adjustedValues[i] = (float)colourValues[i]/(float)colourValues[3]; + percentageError[i] = (adjustedValues[i] - kAverageRedBlock[i]) / kAverageRedBlock[i]; + if ((percentageError[i] < 0 && std::abs(percentageError[i]) < currentMinError[i] * errorMultiplier) || percentageError[i] == 0 || (percentageError[i] > 0 && percentageError[i] < currentMaxError[i] * errorMultiplier)) { + isHazColour[i] = true; + } + } + // for (int i = 0; i < 3; i++) { // if (colourValues[i] < HazBlock->maxColour.components[i] && colourValues[i] > HazBlock->minColour.components[i]) { // isHazColour[i] = true; @@ -491,12 +514,12 @@ button = readSwitches(); // gToggleServoNumber: 1 = Toggle top servo, 2 = Toggle bottom servo, 3 = Toggle both servos - if (gToggleSeroNumber == 1){ + if (gToggleServoNumber == 1){ button = 1; gToggleServoNumber = 0; }else if (gToggleServoNumber == 2){ button = 2; - gTogglerServoNumber = 0; + gToggleServoNumber = 0; } if (button == 1) { @@ -590,6 +613,7 @@ for (int i = 0; i < 3; i++){ weightedValues[i] = (float)colourValues[i] / (float)colourValues[3]; } + //TODO: Print values on LCD lcd->cls(); }