File content as of revision 19:61b21ac4896e:

#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;
PCModes currentMode = None;
Controls currentState = Pause;

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:MBED connected to PC.\n");
}

void disconnectToPC(CommandTypeRaw typeRaw)
{
	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';

		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(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]);
	}
}

void getCurrentBlock(CommandTypeRaw typeRaw)
{
	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);
}

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(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);
}