File content as of revision 30:c0bc92d009fe:

#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::Small, Block::Red);
Block _HazBlock = Block(defaultHazBlock);

bool connectedToPC = false;
bool runServoTest = false;
bool runBreakBeamTest = false;
bool runColourSensorTest = false;
bool getColourSensorValue = false;
bool getBlockColourValue = false;
bool setNewHazBlock = 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] = kMaxRedError[i];
		currentMinError[i] = kMinRedError[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) {
    	setNewHazBlock = true;
    	pc.printf("INFO: Setting new haz block.\n");
	} else if (typeRaw == Query) {
    	pc.printf("VALUE:Hazardous Block:\n \tSize:%i \n \tMin Error:%.3f, %.3f, %.3f\n \t Max Error:%.3f, %.3f, %.3f\n:VALUE", _HazBlock.size, kMinError[_HazBlock.colour][1], kMinError[_HazBlock.colour][1], kMinError[_HazBlock.colour][2], kMaxError[_HazBlock.colour][0], kMaxError[_HazBlock.colour][1], kMaxError[_HazBlock.colour][2]);
        pc.printf("VALUE:\tAverage Colour:%.3f, %.3f, %.3f, %.3f\n:VALUE", kAverageValues[_HazBlock.colour][0], kAverageValues[_HazBlock.colour][1], kAverageValues[_HazBlock.colour][2], kAverageValues[_HazBlock.colour][3]);
    }
}

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 %.2f.\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;
	}
}