![](/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: globals.cpp
- Revision:
- 20:4e0f0944f28f
- Parent:
- 19:61b21ac4896e
- Child:
- 22:993821a4c396
diff -r 61b21ac4896e -r 4e0f0944f28f globals.cpp --- a/globals.cpp Tue Nov 24 01:38:46 2015 +0000 +++ b/globals.cpp Tue Nov 24 20:53:51 2015 +0000 @@ -6,150 +6,188 @@ #include "WattBob_TextLCD.h" int kDefaultBaudRate = 19200; -int ColourSensorError = 25; +//TODO: Not let it be constant. +int ColourSensorError = 0.5; //SerialBase gParity = SerialBase::None; int gStopBits = 1; -Block _HazBlock = Block(kDefaultHazBlock); + +Block _HazBlock = Block(); bool connectedToPC = false; bool runServoTest = false; PCModes currentMode = None; Controls currentState = Pause; +void DefaultHazBlock(){ + Colour _minRedBlock = Colour(); + Colour _maxRedBlock = Colour(); + Colour _averageRedBlock = Colour(); + + for (int i = 0; i < 4; i++){ + _minRedBlock.components[i] = kMinRedBlock[i]; + _maxRedBlock.components[i] = kMaxRedBlock[i]; + _averageRedBlock.components[i] = kAverageRedBlock[i]; + } + + _HazBlock.minColour = Colour(_minRedBlock); + _HazBlock.maxColour = Colour(_maxRedBlock); + _HazBlock.averageColour = Colour(_averageRedBlock); + _HazBlock.size = Block::Small; + +// defaultHazBlock.minColour = Colour(_minRedBlock); +// defaultHazBlock.maxColour = Colour(_maxRedBlock); +// defaultHazBlock.averageColour = Colour(_averageRedBlock); + +// Block _HazBlock = Block(defaultHazBlock); +} + int readSwitches() { - if(i2cport->read_bit(8)) { - while (i2cport->read_bit(8)) { } - return 1; - } else if (i2cport->read_bit(9)) { - while (i2cport->read_bit(9)) { } - return 2; - } else if (i2cport->read_bit(10)) { - while (i2cport->read_bit(10)) { } - return 3; - } else if (i2cport->read_bit(11)) { - while (i2cport->read_bit(11)) { } - return 4; - } else { - return 0; - } + if(i2cport->read_bit(8)) { + while (i2cport->read_bit(8)) { } + return 1; + } else if (i2cport->read_bit(9)) { + while (i2cport->read_bit(9)) { } + return 2; + } else if (i2cport->read_bit(10)) { + while (i2cport->read_bit(10)) { } + return 3; + } else if (i2cport->read_bit(11)) { + while (i2cport->read_bit(11)) { } + return 4; + } else { + return 0; + } } void connectToPC(CommandTypeRaw typeRaw) { - connectedToPC = true; - pc.printf("INFO:MBED connected to PC.\n"); + connectedToPC = true; + pc.printf("INFO:MBED connected to PC.\n"); } void disconnectToPC(CommandTypeRaw typeRaw) { - pc.printf("INFO:MBED disconnected from PC.\n"); - connectedToPC = false; - currentMode = None; + pc.printf("INFO:MBED disconnected from PC.\n"); + connectedToPC = false; + currentMode = None; } void hazBlock(CommandTypeRaw typeRaw) { - if (typeRaw == Set) { - pc.printf("INFO:Setting new haz block.\n"); - int lowerBeam = 0; - int higherBeam = 0; - char readyChar = '\0'; + if (typeRaw == Set) { + pc.printf("INFO:Setting new haz block.\n"); + int lowerBeam = 0; + int higherBeam = 0; + char readyChar = '\0'; + + lcd->cls(); + lcd->locate(0,0); + lcd->printf("New haz block"); - do { higherBeam = fpga->checkForSize(); - } while (higherBeam != 1); + do { + higherBeam = fpga->getBeamValue(Top); + if (readSwitches() == 4) { + if (displayAbortDialog()) { + pc.printf("INFO: Operation aborted form MBED.\n"); + displayPCStatus(); + return; + } else { + lcd->cls(); + lcd->locate(0,0); + lcd->printf("New haz block"); + } - int colourValue[4]; - rgbSensor.getAllColors(colourValue); + } + } while (higherBeam != 1); - do { - // readyChar = pc.getc(); - lowerBeam = fpga->checkForBlock(); - // TODO: Check beam reading - // if (readyChar != 'y'){ - // pc.printf("No block detected by beam\nTry again."); - // } - } while (lowerBeam != 1); - higherBeam = fpga->checkForSize(); + int colourValue[4]; + rgbSensor.getAllColors(colourValue); + + do { + lowerBeam = fpga->checkForBlock(); + } while (lowerBeam != 1); + higherBeam = fpga->checkForSize(); - _HazBlock = Block(); - if (higherBeam == 1) - _HazBlock.size = Block::Big; - else if (higherBeam == 0) - _HazBlock.size = Block::Small; - for (int i = 0; i < 4; i++) { - _HazBlock.minColour.components[i] = colourValue[i] - ColourSensorError; - _HazBlock.maxColour.components[i] = colourValue[i] + ColourSensorError; - } - fpga->moveSortingServo(Haz); - fpga->moveStoppingServo(Go); - while (fpga->checkForBlock()) {} - fpga->moveStoppingServo(Stop); - fpga->moveSortingServo(NonHaz); + _HazBlock = Block(); + if (higherBeam == 1) + _HazBlock.size = Block::Big; + else if (higherBeam == 0) + _HazBlock.size = Block::Small; + for (int i = 0; i < 3; i++) { + _HazBlock.minColour.components[i] = colourValue[i]/colourValue[3] - ColourSensorError; + _HazBlock.maxColour.components[i] = colourValue[i]/colourValue[3] + ColourSensorError; + } + fpga->moveSortingServo(Haz); + fpga->moveStoppingServo(Go); + while (fpga->checkForBlock()) {} + fpga->moveStoppingServo(Stop); + fpga->moveSortingServo(NonHaz); - pc.printf( "HazBlock:\n \t Size:%i\n \t Real Colour:%i,%i,%i,%i\n \tMin Colour:%i,%i,%i,%i\n \t Max Colour:%i,%i,%i,%i\n ", _HazBlock.size, colourValue[0], colourValue[1], colourValue[2], colourValue[3], _HazBlock.minColour.components[Colour::Red], _HazBlock.minColour.components[Colour::Blue], _HazBlock.minColour.components[Colour::Green], _HazBlock.minColour.components[Colour::Alpha], _HazBlock.maxColour.components[Colour::Red], _HazBlock.maxColour.components[Colour::Blue], _HazBlock.maxColour.components[Colour::Green], _HazBlock.maxColour.components[Colour::Alpha]); - } else if (typeRaw == Query) { - pc.printf( "HazBlock:\n \t Size:%i\n \t Min Colour:%i,%i,%i,%i\n \t Max Colour:%i,%i,%i,%i\n ", _HazBlock.size, _HazBlock.minColour.components[Colour::Red], _HazBlock.minColour.components[Colour::Blue], _HazBlock.minColour.components[Colour::Green], _HazBlock.minColour.components[Colour::Alpha], _HazBlock.maxColour.components[Colour::Red], _HazBlock.maxColour.components[Colour::Blue], _HazBlock.maxColour.components[Colour::Green], _HazBlock.maxColour.components[Colour::Alpha]); - } + pc.printf( "VALEU:HazBlock:\n \t Size:%i\n \t Real Colour:%i,%i,%i,%i\n \tMin Colour:%i,%i,%i,%i\n \t Max Colour:%i,%i,%i,%i:VALUE", _HazBlock.size, colourValue[0], colourValue[1], colourValue[2], colourValue[3], _HazBlock.minColour.components[Colour::Red], _HazBlock.minColour.components[Colour::Blue], _HazBlock.minColour.components[Colour::Green], _HazBlock.minColour.components[Colour::Alpha], _HazBlock.maxColour.components[Colour::Red], _HazBlock.maxColour.components[Colour::Blue], _HazBlock.maxColour.components[Colour::Green], _HazBlock.maxColour.components[Colour::Alpha]); + } else if (typeRaw == Query) { + pc.printf( "VALUE:HazBlock:\n \t Size:%i\n \t Min Colour:%i,%i,%i,%i\n \t Max Colour:%i,%i,%i,%i:VALUE", _HazBlock.size, _HazBlock.minColour.components[Colour::Red], _HazBlock.minColour.components[Colour::Blue], _HazBlock.minColour.components[Colour::Green], _HazBlock.minColour.components[Colour::Alpha], _HazBlock.maxColour.components[Colour::Red], _HazBlock.maxColour.components[Colour::Blue], _HazBlock.maxColour.components[Colour::Green], _HazBlock.maxColour.components[Colour::Alpha]); + } } void getCurrentBlock(CommandTypeRaw typeRaw) { - pc.printf("DEBUG: Getting current block readings\n"); + pc.printf("DEBUG: Getting current block readings\n"); } void setIntegrationTimeTo(float integrationTime) { - rgbSensor.setIntegrationTime(integrationTime); - pc.printf("DEBUG: Setting integration-time to %i.\n", integrationTime); + rgbSensor.setIntegrationTime(integrationTime); + pc.printf("DEBUG: Setting integration-time to %i.\n", integrationTime); } void previewOnPC(bool on) { - pc.printf("setting preview on pc to %i.\n", on); + pc.printf("setting preview on pc to %i.\n", on); } -void readColourSensor(){ - int colourValue[4]; - rgbSensor.getAllColors(colourValue); - pc.printf( "Colour Reading:%i,%i,%i,%i\n", colourValue[0], colourValue[1], colourValue[2], colourValue[3]); +void readColourSensor() +{ + int colourValue[4]; + rgbSensor.getAllColors(colourValue); + pc.printf( "VALEU:Colour Reading:%i,%i,%i,%i\n:VALUE", colourValue[0], colourValue[1], colourValue[2], colourValue[3]); } - void testServos(Controls state) { - if (state == Start){ - pc.printf("INFO: Running servo test.\n"); - runServoTest = true; - }else if (state == Pause){ - pc.printf("INFO: Finished running servo test.\n"); - runServoTest = false; - } + if (state == Start) { + pc.printf("INFO: Running servo test.\n"); + runServoTest = true; + } else if (state == Pause) { + pc.printf("INFO: Finished running servo test.\n"); + runServoTest = false; + } } void resetServos() { - pc.printf("resetting servos.\n"); + pc.printf("resetting servos.\n"); } void getPortInfo() { - pc.printf("getting port info.\n"); + pc.printf("getting port info.\n"); } void setPortBaudRate(int baudRate) { - pc.baud(baudRate); - wait(0.1); - pc.printf("DEBUG: Setting port Baud Rate to: %i.\n", baudRate); + pc.baud(baudRate); + wait(0.1); + pc.printf("DEBUG: Setting port Baud Rate to: %i.\n", baudRate); } -void setPortParity(int parity){ - SerialBase::Parity _parity = static_cast<SerialBase::Parity>(parity); - pc.format(8, _parity, 1); - wait(0.1); - pc.printf("DEBUG: Setting port parity to: %i.\n", parity); +void setPortParity(int parity) +{ + SerialBase::Parity _parity = static_cast<SerialBase::Parity>(parity); + pc.format(8, _parity, 1); + wait(0.1); + pc.printf("DEBUG: Setting port parity to: %i.\n", parity); }