File content as of revision 7:10e1ec0e8179:

#include "mbed.h"
#include "m3pi.h"
#include <ADJDColourSensor.h>

#define RED 97 //((const unsigned char *)"1")
#define GREEN 98 //((const unsigned char *) "2")
#define BLUE 99 //((const unsigned char *) "3")
#define BROWN 100 //((cont unsigned char *) "4")
#define BLACK 101 // ((const unsigned char *) "5")
#define YELLOW 102
#define WHITE 103
#define TEST 104 // ((const unsigned char *) "6")
#define SET_PARAMETERS 105
#define GET_PARAMETERS 106
#define OPTIMIZE_PARAMETERS 107
#define GET_TIMING 108
#define NUM_OF_WEIGHTS 7
#define NUM_OF_TESTS 25

//DigitalOut sensorLED(p8);
//I2C colorSensor(p28, p27); //SDA, SCL
Serial bt(p13, p14); // tx, rx
ADJDColourSensor colorSensor(p28, p27, p8);

//const int addr_write = 0x74 << 1;
//const int addr_read = addr_write + 0x1;
m3pi m3pi;

void normalize(double* feature) {
    double magnitude = sqrt(feature[0] * feature[0] + feature[1] * feature[1] + feature[2] * feature[2] + feature[3] * feature[3]);
    feature[0] /= magnitude;
    feature[1] /= magnitude;
    feature[2] /= magnitude;
    feature[3] /= magnitude;
}

void takeMeasurements(int numberOfTests) {
    int redTotal = 0;
    int greenTotal = 0;
    int blueTotal = 0;
    int clearTotal = 0;
    for(int i = 0; i < numberOfTests; i++) {
        colorSensor.readColors();
        redTotal += colorSensor.red();
        greenTotal += colorSensor.green();
        blueTotal += colorSensor.blue();
        clearTotal += colorSensor.clear();
    }
    bt.printf("R:%d, G:%d, B:%d, C:%d\r", redTotal / numberOfTests, greenTotal / numberOfTests, blueTotal / numberOfTests, clearTotal / numberOfTests);
}

double max(double* values) {
    double currentMax = values[0];
    
    for(int i = 1; i < NUM_OF_WEIGHTS; i++) {
        if(values[i] > currentMax)
            currentMax = values[i];
    }
    
    return currentMax;
    
}

int max(int* values) {
    int currentMax = values[0];
    
    for(int i = 1; i < NUM_OF_WEIGHTS; i++) {
        if(values[i] > currentMax)
            currentMax = values[i];
    }
    
    return currentMax;
    
}

void correctWeights(double* wrongWeights, double* properWeights) {
    double colors[] = { (double) colorSensor.red(), (double) colorSensor.green(), (double) colorSensor.blue() };
    normalize(colors);
    wrongWeights[0] -= colors[0]; //colorSensor.red();
    wrongWeights[1] -= colors[1]; //colorSensor.green();
    wrongWeights[2] -= colors[2]; //colorSensor.blue();
    properWeights[0] += colors[0]; //colorSensor.red();
    properWeights[1] += colors[1]; //colorSensor.green();
    properWeights[2] += colors[2]; //colorSensor.blue();
    
    normalize(wrongWeights);
    normalize(properWeights);
}

void correctWeights(int* wrongWeights, int* properWeights) {
    wrongWeights[0] -= colorSensor.red();
    wrongWeights[1] -= colorSensor.green();
    wrongWeights[2] -= colorSensor.blue();
    properWeights[0] += colorSensor.red();
    properWeights[1] += colorSensor.green();
    properWeights[2] += colorSensor.blue();
}


void sendData(char* color, double* redWeights, double* greenWeights, double* blueWeights, double* brownWeights, double* blackWeights, double* yellowWeights, double* whiteWeights) {
    bt.printf("%s,%d,%d,%d,%d,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\r", color, colorSensor.red(), colorSensor.green(), colorSensor.blue(), colorSensor.clear(),redWeights[0], redWeights[1], redWeights[2], redWeights[3], greenWeights[0], greenWeights[1], greenWeights[2], greenWeights[3], blueWeights[0], blueWeights[1], blueWeights[2], blueWeights[3], brownWeights[0], brownWeights[1], brownWeights[2], brownWeights[3], blackWeights[0], blackWeights[1], blackWeights[2], blackWeights[3], yellowWeights[0], yellowWeights[1], yellowWeights[2], yellowWeights[3], whiteWeights[0], whiteWeights[1], whiteWeights[2], whiteWeights[3]);
}

void sendData(char* color, int* redWeights, int* greenWeights, int* blueWeights, int* brownWeights, int* blackWeights, int* yellowWeights, int* whiteWeights) {
    bt.printf("%s,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\r", color, colorSensor.red(), colorSensor.green(), colorSensor.blue(), colorSensor.clear(),redWeights[0], redWeights[1], redWeights[2], redWeights[3], greenWeights[0], greenWeights[1], greenWeights[2], greenWeights[3], blueWeights[0], blueWeights[1], blueWeights[2], blueWeights[3], brownWeights[0], brownWeights[1], brownWeights[2], brownWeights[3], blackWeights[0], blackWeights[1], blackWeights[2], blackWeights[3], yellowWeights[0], yellowWeights[1], yellowWeights[2], yellowWeights[3], whiteWeights[0], whiteWeights[1], whiteWeights[3], whiteWeights[3]);
}

void calibrate(double* weights) {
    int redTotal = 0;
    int greenTotal = 0;
    int blueTotal = 0;
    int clearTotal = 0;
    for(int i = 0; i < NUM_OF_TESTS; i++) {
        colorSensor.readColors();
        redTotal += colorSensor.red();
        greenTotal += colorSensor.green();
        blueTotal += colorSensor.blue();
        clearTotal += colorSensor.clear();
    }
    weights[0] = (double) redTotal;
    weights[1] = (double) greenTotal;
    weights[2] = (double) blueTotal;
    weights[3] = (double) clearTotal;
    normalize(weights);


}

/*
void calibrate(int* redWeights, int* greenWeights, int* blueWeights, int* brownWeights, int* blackWeights, int* desiredWeights) {
    colorSensor.readColors();
    int redProduct = colorSensor.dotProduct(redWeights);
    int greenProduct = colorSensor.dotProduct(greenWeights);
    int blueProduct = colorSensor.dotProduct(blueWeights);
    int brownProduct = colorSensor.dotProduct(brownWeights);
    int blackProduct = colorSensor.dotProduct(blackWeights);
    int products[] = {redProduct, greenProduct, blueProduct, brownProduct, blackProduct};
    int maxProduct = max(products);
    char* color;
    if(colorSensor.dotProduct(desiredWeights) == maxProduct) {
        if(redWeights == desiredWeights) {
            color = "Red";
        } else if(greenWeights == desiredWeights) {
            color = "Green";
        } else if(blueWeights == desiredWeights) {
            color = "Blue";
        } else if(brownWeights == desiredWeights) {
            color = "Brown";
        } else if(blackWeights == desiredWeights) {
            color = "Black";
        }
    } else if(redProduct == maxProduct) {
        correctWeights(redWeights, desiredWeights);
        color = "Red";
    } else if(greenProduct == maxProduct) {
        correctWeights(greenWeights, desiredWeights);
        color = "Green";
    } else if(blueProduct == maxProduct) {
        correctWeights(blueWeights, desiredWeights);
        color = "Blue";
    } else if(brownProduct == maxProduct) {
        correctWeights(brownWeights, desiredWeights);
        color = "Brown";
    } else if(blackProduct == maxProduct) {
        correctWeights(blackWeights, desiredWeights);
        color = "Black";
    }
    //sendData(color, redWeights, greenWeights, blueWeights, brownWeights, blackWeights);
} */

void measure(double* redWeights, double* greenWeights, double* blueWeights, double* brownWeights, double* blackWeights, double* yellowWeights, double* whiteWeights) {
    colorSensor.readColors();
    double redProduct = colorSensor.dotProduct(redWeights);
    double greenProduct = colorSensor.dotProduct(greenWeights);
    double blueProduct = colorSensor.dotProduct(blueWeights);
    double brownProduct = colorSensor.dotProduct(brownWeights);
    double blackProduct = colorSensor.dotProduct(blackWeights);
    double yellowProduct = colorSensor.dotProduct(yellowWeights);
    double whiteProduct = colorSensor.dotProduct(whiteWeights);
    double products[] = {redProduct, greenProduct, blueProduct, brownProduct, blackProduct, yellowProduct, whiteProduct};
    double maxProduct = max(products);
    char* color;
    if(redProduct == maxProduct) {
        color = "Red";
    } else if(greenProduct == maxProduct) {
        color = "Green";
    } else if(blueProduct == maxProduct) {
        color = "Blue";
    } else if(brownProduct == maxProduct) {
        color = "Brown";
    } else if(blackProduct == maxProduct) {
        color = "Black";
    } else if(yellowProduct == maxProduct) {
        color = "Yellow";
    } else if(whiteProduct == maxProduct) {
        color = "White";
    }
    sendData(color, redWeights, greenWeights, blueWeights, brownWeights, blackWeights, yellowWeights, whiteWeights);
}


void measure(int* redWeights, int* greenWeights, int* blueWeights, int* brownWeights, int* blackWeights, int* yellowWeights, int* whiteWeights) {
    colorSensor.readColors();
    int redProduct = colorSensor.dotProduct(redWeights);
    int greenProduct = colorSensor.dotProduct(greenWeights);
    int blueProduct = colorSensor.dotProduct(blueWeights);
    int brownProduct = colorSensor.dotProduct(brownWeights);
    int blackProduct = colorSensor.dotProduct(blackWeights);
    int yellowProduct = colorSensor.dotProduct(yellowWeights);
    int whiteProduct = colorSensor.dotProduct(whiteWeights);
    int products[] = {redProduct, greenProduct, blueProduct, brownProduct, blackProduct, yellowProduct, whiteProduct};
    int maxProduct = max(products);
    char* color;
    if(redProduct == maxProduct) {
        color = "Red";
    } else if(greenProduct == maxProduct) {
        color = "Green";
    } else if(blueProduct == maxProduct) {
        color = "Blue";
    } else if(brownProduct == maxProduct) {
        color = "Brown";
    } else if(blackProduct == maxProduct) {
        color = "Black";
    } else if(yellowProduct == maxProduct) {
        color = "Yellow";
    } else if(whiteProduct == maxProduct) {
        color = "White";
    }
    sendData(color, redWeights, greenWeights, blueWeights, brownWeights, blackWeights, yellowWeights, whiteWeights);
}

void setParameters() {
    int r_cap, g_cap, b_cap, c_cap, r_int, g_int, b_int, c_int;
    bt.scanf("%d,%d,%d,%d,%x,%x,%x,%x\r", &r_cap, &g_cap, &b_cap, &c_cap, &r_int, &g_int, &b_int, &c_int);
    colorSensor.setCapacitors(r_cap, g_cap, b_cap, c_cap);
    colorSensor.setIntegrationTimeSlot(r_int, g_int, b_int, c_int);
}

void getParameters() {
    bt.printf("%d,%d,%d,%d,%x,%x,%x,%x\r", colorSensor.redCap(), colorSensor.greenCap(), colorSensor.blueCap(), colorSensor.clearCap(), colorSensor.redInt(), colorSensor.greenInt(), colorSensor.blueInt(), colorSensor.clearInt());
}

void optimizeParameters() {
    int redDes, greenDes, blueDes, clearDes;
    bt.scanf("%d,%d,%d,%d\r", &redDes, &greenDes, &blueDes, &clearDes);
    colorSensor.optimise(redDes, greenDes, blueDes, clearDes);
}


int main() {

    bt.baud(115200);
    colorSensor.setIntegrationTimeSlot(0xff, 0xf0, 0xff, 0xff);
    colorSensor.setCapacitors(12, 7, 2, 9);
    //colorSensor.setIntegrationTimeSlot(0xff, 0x110, 0x104, 0xff);
    //colorSensor.setCapacitors(7, 7, 7, 7);
    
    
    /*
    int redWeights[] = {100, -50, -50};
    int greenWeights[] = {-50, 100, -50};
    int blueWeights[] = {-50, -50, 100};
    int brownWeights[] = {1, 0, 1};
    int blackWeights[] = {1, 1, 1};
    double redWeights[] = {311.4, 233.2, 217.6};
    */
    double redWeights[] = {318.8, 242.6, 227.4, 712.4};
    double greenWeights[] = {206.2, 258.2, 242.6, 640.4};
    double blueWeights[] = {190.8, 223.4, 307.6, 630.8};
    double brownWeights[] = {-205.6, -215.0, -212.4, -588.0};
    double blackWeights[] = {175.6, 187.6, 196.8, 530.4};
    double yellowWeights[] = {480.8, 542.6, 315.8, 1021.2};
    double whiteWeights[] = {455.6, 584.8, 578.8, 1021.8};
    
    
    normalize(redWeights); normalize(greenWeights); normalize(blueWeights); normalize(brownWeights); normalize(blackWeights); normalize(yellowWeights); normalize(whiteWeights);
    int message;
    
    while(1) {
        //takeMeasurements(20);
        
        
        message = bt.getc();
        //bt.printf("\r%d\r", message);
        
        switch(message) {
        case RED:
            calibrate(redWeights);
            sendData("", redWeights, greenWeights, blueWeights, brownWeights, blackWeights, yellowWeights, whiteWeights);
            break;
        case GREEN:
            calibrate(greenWeights);
            sendData("", redWeights, greenWeights, blueWeights, brownWeights, blackWeights, yellowWeights, whiteWeights);
            break;
        case BLUE:
            calibrate(blueWeights);
            sendData("", redWeights, greenWeights, blueWeights, brownWeights, blackWeights, yellowWeights, whiteWeights);
            break;
        case BROWN:
            calibrate(brownWeights);
            sendData("", redWeights, greenWeights, blueWeights, brownWeights, blackWeights, yellowWeights, whiteWeights);
            break;
        case BLACK:
            calibrate(blackWeights);
            sendData("", redWeights, greenWeights, blueWeights, brownWeights, blackWeights, yellowWeights, whiteWeights);
            break;
        case YELLOW:
            calibrate(yellowWeights);
            sendData("", redWeights, greenWeights, blueWeights, brownWeights, blackWeights, yellowWeights, whiteWeights);
            break;
        case WHITE:
            calibrate(whiteWeights);
            sendData("", redWeights, greenWeights, blueWeights, brownWeights, blackWeights, yellowWeights, whiteWeights);
            break;
        case TEST:
            measure(redWeights,greenWeights, blueWeights, brownWeights, blackWeights, yellowWeights, whiteWeights);
            break;
        case SET_PARAMETERS:
            setParameters();
            break;
        case GET_PARAMETERS:
            getParameters();
            break;
        case OPTIMIZE_PARAMETERS:
            optimizeParameters();
            break;
        default:
            break;
        } 
    }
    
}