File content as of revision 29:9c0339e3c593:

#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) {
trySetHazBlockAgain:
		pc.printf("NHZB:Size:%i,Colour:%i;", _HazBlock.size, _HazBlock.colour);
        fpga->moveSortingServo(Haz);
        pc.printf("INFO:Setting new haz block.\n");
        int lowerBeam = 0;
        int higherBeam = 0;
        int colourValues[6][4];
        for (int i = 0; i < 6; i++){
        	memset(colourValues[i], 0, sizeof(colourValues[6]));
        }
        int readingCount = 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 && setNewHazBlock == true);

        if (setNewHazBlock == false) {
        	displayPCStatus();
        	return;
        }
        
        do {
        	rgbSensor.getAllColors(colourValues[readingCount]);
        	readingCount++;
        } while (readingCount < 6 && fpga->getBeamValue(Top) == 1 && fpga->getBeamValue(Bottom) == 0);

        lowerBeam = fpga->getBeamValue(Bottom);
        higherBeam = fpga->getBeamValue(Top);
        if (lowerBeam != 1){
        	lowerBeam = fpga->getBeamValue(Top);
        	while(lowerBeam != 1) { lowerBeam = fpga->getBeamValue(Bottom); }
        	higherBeam = fpga->getBeamValue(Bottom);
        }
        Block::Size blockSize = higherBeam;

        int totalComponents[3];
        memset(totalComponents, 0, sizeof(totalComponents));
        for (int k = 0; k < readingCount; k++){
        	for (int i = 0; i < 4; i++){
        		totalComponents[i] += colourValues[k][i];
        	}
        }
        float averageComponents[3];
        memset(averageComponents, 0, sizeof(averageComponents));

        for (int i = 0; i < 4; i++){
        	averageComponents = (float)totalComponents[i] / (float)readingCount;
        }

        for (int i = 0; i < 3; i++){
        	adjustedValues[i] = averageComponents[i] / averageComponents[i];
        }

        Block::BlockColour detectedColour = Block::Wrong;
        bool matchesColour[3] = {false, false, false};

        for (int k = 0; k < 7; k++){
        	for (int i = 0; i < 3; i++) {
        		percentageError[i] = (adjustedValues[i] - kAverageValues[HazBlock->colour][i]) / kAverageValues[HazBlock->colour][i];

        		if ((percentageError[i] < 0 && std::abs(percentageError[i]) < kMinError[HazBlock->colour][i] * errorMultiplier) || percentageError[i] == 0 || (percentageError[i] > 0 && percentageError[i] < kMaxError[HazBlock->colour][i] * errorMultiplier)) {
        			matchesColour[i] = true;
        		}
        		if (matchesColour[0] && matchesColour[1] && matchesColour[2]){
        			detectedColour = k;
        			break;
        		}
        	}
        }

        if (detectedColour != Block::Wrong){
        	pc.printf("ERROR: Could not detect colour.\n");
        	lcd->cls();
        	lcd->printf("1: Try again");
        	lcd->locate(1,0);
        	lcd->printf("2: Revert to last");
        	int button = 0;
        	do {
        		button = readSwitches();
        		if (button == 1){
        			goto trySetHazBlockAgain;
        		}
        	} while (button != 2);
        }

        // Point and literal might not sync...
        _HazBlock.size = blockSize;
        _HazBlock.colour = detectedColour;

        pc.printf("NHZB:Size:%i,Colour:%i;", _HazBlock.size, _HazBlock.colour);


        pc.printf("VALUE:Hazardous Block:\n \tSize:%i \n \tMin Error:%i, %i, %i\n \t Max Error:%i, %i, %i\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]);
        fpga->moveSortingServo(NonHaz);
    } else if (typeRaw == Query) {
    	pc.printf("VALUE:Hazardous Block:\n \tSize:%i \n \tMin Error:%i, %i, %i\n \t Max Error:%i, %i, %i\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 %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;
	}
}