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