File content as of revision 10:16ba52f8e025:

#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;

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 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;
        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);
		
        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 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 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 testServos()
{
    runServoTest = true;
}

void resetServos()
{
    pc.printf("resetting servos.\n");
}

void getPortInfo()
{
    pc.printf("getting port info.\n");
}

void setPortBaudRate(int baudRate)
{
    pc.printf("setting port baudrate to: %i\n", baudRate);
}