![](/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:
- 10:16ba52f8e025
- Parent:
- 9:dc8f155b71c8
- Child:
- 11:0fe833f8a1ab
diff -r dc8f155b71c8 -r 16ba52f8e025 globals.cpp --- a/globals.cpp Mon Nov 16 23:44:44 2015 +0000 +++ b/globals.cpp Wed Nov 18 16:09:52 2015 +0000 @@ -1,80 +1,139 @@ #include "globals.h" #include "Block.h" #include "fpga.h" +#include "TCS3472_I2C.h" +#include "MCP23017.h" +#include "WattBob_TextLCD.h" int kDefaultBaudRate = 19200; +int ColourSensorError = 25; //SerialBase gParity = SerialBase::None; int gStopBits = 1; Block _HazBlock = Block(kDefaultHazBlock); bool connectedToPC = false; +bool runServoTest = false; -void connectToPC(CommandTypeRaw typeRaw){ - pc.printf("pc told to connect\n"); - connectedToPC = true; +int readSwitches() +{ + + if(i2cport->read_bit(8)) { + return 1; + } else if (i2cport->read_bit(9)) { + return 2; + } else if (i2cport->read_bit(10)) { + return 3; + } else if (i2cport->read_bit(11)) { + return 4; + } else { + return 0; + } + } -void disconnectToPC(CommandTypeRaw typeRaw){ - pc.printf("pc told to disconnect\n"); - connectedToPC = false; +void connectToPC(CommandTypeRaw typeRaw) +{ + pc.printf("pc told to connect\n"); + connectedToPC = true; +} + +void disconnectToPC(CommandTypeRaw typeRaw) +{ + pc.printf("pc told to disconnect\n"); + connectedToPC = false; } -void hazBlock(CommandTypeRaw typeRaw){ - if (typeRaw == Set) { - pc.printf("setting new haz block.\n"); - pc.printf("Send 'y' when block is inserted.\n"); - int lowerBeam = 0; - char readyChar = '\0'; - do{ - readyChar = pc.getc(); - lowerBeam = fpga->checkForBlock(); - if (readyChar == 'y' && lowerBeam == 0){ - pc.printf("No block detected by beam\nTry again."); - } - } while (readyChar != 'y' || lowerBeam != 0); +void hazBlock(CommandTypeRaw typeRaw) +{ + if (typeRaw == Set) { + pc.printf("setting new haz block.\n"); +// pc.printf("Send 'y' when block is inserted.\n"); + int lowerBeam = 0; + int higherBeam = 0; + char readyChar = '\0'; + + do { higherBeam = fpga->checkForSize(); + } while (higherBeam != 1); + + int colourValue[4]; + rgbSensor.getAllColors(colourValue); + + 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(); + + + + + _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(Go); + fpga->moveSortingServo(NonHaz); - int higherBeam = 0; - higherBeam = fpga->checkForSize(); - - _HazBlock = Block(); - if (higherBeam == 1) - _HazBlock.size = Block::Big; - else if (higherBeam == 0) - _HazBlock.size = Block::Small; - - - - }else if (typeRaw == Query){ - pc.printf( "HazBlock:\n \t Size:%i\n \t Min Colour:%i,%i,%i,%i\n \t Max Colour:TBA\n ", _HazBlock.size, _HazBlock.minColour.components[Colour::Red], _HazBlock.minColour.components[Colour::Blue], _HazBlock.minColour.components[Colour::Green], _HazBlock.minColour.components[Colour::Alpha]); - } + 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]); + } } -void getCurrentBlock(CommandTypeRaw typeRaw){ - pc.printf("getting current block readings\n"); +void getCurrentBlock(CommandTypeRaw typeRaw) +{ + pc.printf("getting current block readings\n"); } -void setIntegrationTime(int integrationTime){ - pc.printf("setting i-time to %i.\n", integrationTime); +void setIntegrationTime(int integrationTime) +{ + rgbSensor.setIntegrationTime(integrationTime); + pc.printf("setting i-time to %i.\n", integrationTime); } -void previewOnPC(bool on){ - pc.printf("setting preview on pc to %i.\n", on); +void previewOnPC(bool on) +{ + pc.printf("setting preview on pc to %i.\n", on); } -void testServos(){ - pc.printf("testing servos.\n"); +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 resetServos(){ - pc.printf("resetting servos.\n"); + +void testServos() +{ + runServoTest = true; } -void getPortInfo(){ - pc.printf("getting port info.\n"); +void resetServos() +{ + pc.printf("resetting servos.\n"); } -void setPortBaudRate(int baudRate){ - pc.printf("setting port baudrate to: %i\n", baudRate); +void getPortInfo() +{ + pc.printf("getting port info.\n"); } +void setPortBaudRate(int baudRate) +{ + pc.printf("setting port baudrate to: %i\n", baudRate); +} +