![](/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:
- 25:792540d69c49
- Parent:
- 24:02c61793f90b
- Child:
- 26:bbcc25418ffa
--- a/main.cpp Sun Nov 29 01:05:42 2015 +0000 +++ b/main.cpp Mon Nov 30 15:06:38 2015 +0000 @@ -55,6 +55,7 @@ void runInBreakBeamTestMode(); void turnOffTopLEDs(); void turnOffBottomLEDs(); +void runInColourSensorTestMode(); int main() { @@ -65,7 +66,7 @@ lcd->cls(); rgbSensor.enablePowerAndRGBC(); - rgbSensor.setIntegrationTime(3); + rgbSensor.setIntegrationTime(gIntegrationTime); // Create a serial intereput for RxIrq so when PC is connected it sends '$' to tell MBED it's there. // https://developer.mbed.org/cookbook/Serial-Interrupts @@ -143,6 +144,8 @@ runInServoTestMode(); else if (runBreakBeamTest == true) runInBreakBeamTestMode(); + else if (runColourSensorTest == true) + runInColourSensorTestMode(); } } else if (currentMode == Normal) { displayPCStatus(); @@ -450,15 +453,15 @@ lcd->cls(); lcd->locate(0,0); if (fpga->stoppingServoPosition == Stop) - lcd->printf("1: Top: Stop"); + lcd->printf("1:Top: Go"); else if (fpga->stoppingServoPosition == Go) - lcd->printf("1: Top: Go"); + lcd->printf("1:Top: Stop"); lcd->locate(1,0); if (fpga->sortingServoPosition == NonHaz) - lcd->printf("2: Bottom: NonHaz"); + lcd->printf("2:Bottom: Haz"); else if (fpga->sortingServoPosition == Haz) - lcd->printf("2: Bottom: Haz"); + lcd->printf("2:Bottom: NonHaz"); } void printServoInfoOnPC() { @@ -486,6 +489,16 @@ bool finished = false; do { button = readSwitches(); + + // gToggleServoNumber: 1 = Toggle top servo, 2 = Toggle bottom servo, 3 = Toggle both servos + if (gToggleSeroNumber == 1){ + button = 1; + gToggleServoNumber = 0; + }else if (gToggleServoNumber == 2){ + button = 2; + gTogglerServoNumber = 0; + } + if (button == 1) { fpga->toggleStoppingServo(); printServoInfoOnLCD(); @@ -497,6 +510,7 @@ printServoInfoOnPC(); wait(kServoWait); } + finished = button == 4; button = 0; } while (finished == false && runServoTest == true); @@ -523,7 +537,7 @@ } void printBeamInfoOnPC(int topBeam, int bottomBeam){ - pc.printf(":<break_beam>1=%i,2=%i;", topBeam, bottomBeam); + pc.printf(":<break_beam>2=%i,1=%i;", topBeam, bottomBeam); } void runInBreakBeamTestMode(){ @@ -570,6 +584,91 @@ return; } +void printColourSensorInfoOnLCD(int colourValues[]){ + float weightedValues[4]; + + for (int i = 0; i < 3; i++){ + weightedValues[i] = (float)colourValues[i] / (float)colourValues[3]; + } + lcd->cls(); +} + +void printColourSensorInfoOnPC(int colourValues[]){ + pc.printf("!<colour_sensor>red=%i,green=%i,blue=%i,clear=%i;", colourValues[0], colourValues[1], colourValues[2], colourValues[3]); +} + +void runInColourSensorTestMode() { + turnOffTopLEDs(); + i2cport->write_bit(1, 15); + i2cport->write_bit(1, 12); + lcd->cls(); + lcd->locate(0,0); + lcd->printf("Colour Sensor"); + lcd->locate(1,0); + lcd->printf("Test Mode"); + pc.printf("!<colour_sensor>i-time=%.3f", gIntegrationTime); + + int button = 0; + bool finished = false; + + do { + button = readSwitches(); + + if (getColourSensorValue == true){ + int colourValues[4]; + rgbSensor.getAllColors(colourValues); + printColourSensorInfoOnPC(colourValues); + printColourSensorInfoOnLCD(colourValues); + getColourSensorValue = false; + } else if (getBlockColourValue == true){ + int topBeam = fpga->getBeamValue(1); + + while(topBeam == 0){ + topBeam = fpga->getBeamValue(1); + } + int colourValues[3][4]; + for (int i = 0; i < 3; i++) { memset(colourValues[i], 0, sizeof(colourValues)); } + int valueCount = 0; + do { + rgbSensor.getAllColors(colourValues[valueCount]); + } while (topBeam == 1 && valueCount < 3); + int averageValues[4] = {0, 0, 0, 0}; + for (int i = 0; i < 3; i++) { + averageValues[0] += colourValues[i][0]; + averageValues[1] += colourValues[i][1]; + averageValues[2] += colourValues[i][2]; + averageValues[3] += colourValues[i][3]; + } + averageValues[0] = averageValues[0] / valueCount; + averageValues[1] = averageValues[1] / valueCount; + averageValues[2] = averageValues[2] / valueCount; + averageValues[3] = averageValues[3] / valueCount; + + printColourSensorInfoOnPC(averageValues); + printColourSensorInfoOnLCD(averageValues); + + getBlockColourValue = false; + } else if (button == 1){ + getColourSensorValue = true; + button = 0; + } + + finished = button == 4; +// button = 0; + } while (finished == false && runColourSensorTest == true); + + turnOffBottomLEDs(); + lcd->cls(); + lcd->locate(0,0); + lcd->printf("Finished test"); + if (runColourSensorTest == true){ + pc.printf(":<colour_sensor>test=pause;"); + runColourSensorTest = false; + } + wait(0.5); + return; +} + void displayWaitingLine() { lcd->cls();