a beter verjin
Dependencies: m3pi mbed ADJD-S371_ColourSens
Fork of TestColorSensor by
main.cpp
- Committer:
- nolanwagener
- Date:
- 2013-12-14
- Revision:
- 7:10e1ec0e8179
- Parent:
- 5:3dd9b0a35aee
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; } } }