![](/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
globals.cpp
- Committer:
- dreamselec
- Date:
- 2015-12-01
- Revision:
- 28:7e4d29977d72
- Parent:
- 27:2cb1bdb7ae3d
- Child:
- 29:9c0339e3c593
File content as of revision 28:7e4d29977d72:
#include "globals.h" #include "Block.h" #include "fpga.h" #include "TCS3472_I2C.h" #include "MCP23017.h" #include "WattBob_TextLCD.h" int kDefaultBaudRate = 19200; //TODO: Not let it be constant. int ColourSensorError = 0.5; //SerialBase gParity = SerialBase::None; int gStopBits = 1; float gIntegrationTime = 2.5; int gToggleServoNumber = 0; float currentMinError[3] = {0,0,0}; float currentMaxError[3] = {0,0,0}; Block defaultHazBlock = Block(); Block _HazBlock = Block(); bool connectedToPC = false; bool runServoTest = false; bool runBreakBeamTest = false; bool runColourSensorTest = false; bool getColourSensorValue = false; bool getBlockColourValue = false; bool pcModeChanged = false; int errorMultiplier = 1; int hazReadingCount = 1; PCModes currentMode = None; Controls currentState = Pause; void DefaultHazBlock(){ for (int i = 0; i < 3; i++){ currentMaxError[i] = kMaxError[i]; currentMinError[i] = kMinError[i]; } 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); defaultHazBlock.size = Block::Small; _HazBlock = Block(defaultHazBlock); // pc.printf( "VALUE:HazBlock:\n \t Size:%i\n \t Min Colour:%f,%f,%f,%f\n \t Max Colour:%f,%f,%f,%f: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 printColourDescription(Colour colour){ pc.printf("Red: %.3f, Green: %.3f, Blue: %.3f, Clear: %.3f\n", colour.components[0], colour.components[1], colour.components[2], colour.components[3]); } void printBlockDescription(Block block){ pc.printf("VALUE:Size: %i\n", block.size); printColourDescription(block.minColour); printColourDescription(block.averageColour); printColourDescription(block.maxColour); //TODO: print errors pc.printf(":VALUE"); } 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; } } void connectToPC(CommandTypeRaw typeRaw) { connectedToPC = true; pc.printf("INFO:PC connected to MBED.\n"); } void disconnectToPC(CommandTypeRaw typeRaw) { pc.printf("INFO:PC disconnected to MBED.\n"); connectedToPC = false; currentMode = None; } void hazBlock(CommandTypeRaw typeRaw) { if (typeRaw == Set) { fpga->moveSortingServo(Haz); 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->getBeamValue(Top); if (readSwitches() == 4) { if (displayAbortDialog()) { //TODO: tell pc pc.printf(":<mbed>haz-block=pause"); pc.printf("INFO: Operation aborted form MBED.\n"); fpga->moveSortingServo(NonHaz); displayPCStatus(); return; } else { lcd->cls(); lcd->locate(0,0); lcd->printf("New haz block"); } } } while (higherBeam != 1); int colourValues[4]; int averageColourValues[4] = {0, 0, 0, 0}; int numberOfReadings = 3; for (int i = 0; i < 4; i++) { rgbSensor.getAllColors(colourValues); for (int j = 0; j < 4; j++) { averageColourValues[j] += colourValues[j]; } } for (int i = 0; i < 4; i++) { averageColourValues[i] = averageColourValues[i] / numberOfReadings; } 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 < 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); //TODO: Fix format. pc.printf( "VALUE:HazBlock:\n \t Size:%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"); } void setIntegrationTimeTo(float integrationTime) { gIntegrationTime = integrationTime; rgbSensor.setIntegrationTime(gIntegrationTime); pc.printf("DEBUG: Setting integration-time to %i.\n", gIntegrationTime); } void previewOnPC(bool on) { pc.printf("setting preview on pc to %i.\n", on); } void testColourSensor(Controls state){ if (state == Start){ pc.printf("INFO: Running colour test.\n"); runColourSensorTest = true; } else if (state == Pause){ pc.printf("INFO: Finished colour test.\n"); runColourSensorTest = false; } } 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; } } void resetServos() { pc.printf("resetting servos.\n"); } void getPortInfo() { 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); } 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 testBreakBeams(Controls state){ if (state == Start){ pc.printf("INFO: Running break beam test.\n"); runBreakBeamTest = true; }else if (state == Pause){ pc.printf("INFO: Exiting break beam test.\n"); runBreakBeamTest = false; } }