a beter verjin
Dependencies: m3pi mbed ADJD-S371_ColourSens
Fork of TestColorSensor by
main.cpp@7:10e1ec0e8179, 2013-12-14 (annotated)
- Committer:
- nolanwagener
- Date:
- Sat Dec 14 08:18:00 2013 +0000
- Revision:
- 7:10e1ec0e8179
- Parent:
- 5:3dd9b0a35aee
additional functions
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
TaintedTruth | 0:1eada136b388 | 1 | #include "mbed.h" |
TaintedTruth | 0:1eada136b388 | 2 | #include "m3pi.h" |
nolanwagener | 5:3dd9b0a35aee | 3 | #include <ADJDColourSensor.h> |
TaintedTruth | 0:1eada136b388 | 4 | |
nolanwagener | 7:10e1ec0e8179 | 5 | #define RED 97 //((const unsigned char *)"1") |
nolanwagener | 7:10e1ec0e8179 | 6 | #define GREEN 98 //((const unsigned char *) "2") |
nolanwagener | 7:10e1ec0e8179 | 7 | #define BLUE 99 //((const unsigned char *) "3") |
nolanwagener | 7:10e1ec0e8179 | 8 | #define BROWN 100 //((cont unsigned char *) "4") |
nolanwagener | 7:10e1ec0e8179 | 9 | #define BLACK 101 // ((const unsigned char *) "5") |
nolanwagener | 7:10e1ec0e8179 | 10 | #define YELLOW 102 |
nolanwagener | 7:10e1ec0e8179 | 11 | #define WHITE 103 |
nolanwagener | 7:10e1ec0e8179 | 12 | #define TEST 104 // ((const unsigned char *) "6") |
nolanwagener | 7:10e1ec0e8179 | 13 | #define SET_PARAMETERS 105 |
nolanwagener | 7:10e1ec0e8179 | 14 | #define GET_PARAMETERS 106 |
nolanwagener | 7:10e1ec0e8179 | 15 | #define OPTIMIZE_PARAMETERS 107 |
nolanwagener | 7:10e1ec0e8179 | 16 | #define GET_TIMING 108 |
nolanwagener | 5:3dd9b0a35aee | 17 | #define NUM_OF_WEIGHTS 7 |
nolanwagener | 5:3dd9b0a35aee | 18 | #define NUM_OF_TESTS 25 |
nolanwagener | 5:3dd9b0a35aee | 19 | |
nolanwagener | 5:3dd9b0a35aee | 20 | //DigitalOut sensorLED(p8); |
nolanwagener | 5:3dd9b0a35aee | 21 | //I2C colorSensor(p28, p27); //SDA, SCL |
TaintedTruth | 2:5c0159599726 | 22 | Serial bt(p13, p14); // tx, rx |
nolanwagener | 5:3dd9b0a35aee | 23 | ADJDColourSensor colorSensor(p28, p27, p8); |
TaintedTruth | 0:1eada136b388 | 24 | |
nolanwagener | 5:3dd9b0a35aee | 25 | //const int addr_write = 0x74 << 1; |
nolanwagener | 5:3dd9b0a35aee | 26 | //const int addr_read = addr_write + 0x1; |
TaintedTruth | 1:d481b2912c2a | 27 | m3pi m3pi; |
TaintedTruth | 1:d481b2912c2a | 28 | |
nolanwagener | 5:3dd9b0a35aee | 29 | void normalize(double* feature) { |
nolanwagener | 5:3dd9b0a35aee | 30 | double magnitude = sqrt(feature[0] * feature[0] + feature[1] * feature[1] + feature[2] * feature[2] + feature[3] * feature[3]); |
nolanwagener | 5:3dd9b0a35aee | 31 | feature[0] /= magnitude; |
nolanwagener | 5:3dd9b0a35aee | 32 | feature[1] /= magnitude; |
nolanwagener | 5:3dd9b0a35aee | 33 | feature[2] /= magnitude; |
nolanwagener | 5:3dd9b0a35aee | 34 | feature[3] /= magnitude; |
nolanwagener | 5:3dd9b0a35aee | 35 | } |
nolanwagener | 5:3dd9b0a35aee | 36 | |
nolanwagener | 5:3dd9b0a35aee | 37 | void takeMeasurements(int numberOfTests) { |
nolanwagener | 5:3dd9b0a35aee | 38 | int redTotal = 0; |
nolanwagener | 5:3dd9b0a35aee | 39 | int greenTotal = 0; |
nolanwagener | 5:3dd9b0a35aee | 40 | int blueTotal = 0; |
nolanwagener | 5:3dd9b0a35aee | 41 | int clearTotal = 0; |
nolanwagener | 5:3dd9b0a35aee | 42 | for(int i = 0; i < numberOfTests; i++) { |
nolanwagener | 5:3dd9b0a35aee | 43 | colorSensor.readColors(); |
nolanwagener | 5:3dd9b0a35aee | 44 | redTotal += colorSensor.red(); |
nolanwagener | 5:3dd9b0a35aee | 45 | greenTotal += colorSensor.green(); |
nolanwagener | 5:3dd9b0a35aee | 46 | blueTotal += colorSensor.blue(); |
nolanwagener | 5:3dd9b0a35aee | 47 | clearTotal += colorSensor.clear(); |
nolanwagener | 5:3dd9b0a35aee | 48 | } |
nolanwagener | 5:3dd9b0a35aee | 49 | bt.printf("R:%d, G:%d, B:%d, C:%d\r", redTotal / numberOfTests, greenTotal / numberOfTests, blueTotal / numberOfTests, clearTotal / numberOfTests); |
nolanwagener | 5:3dd9b0a35aee | 50 | } |
nolanwagener | 5:3dd9b0a35aee | 51 | |
nolanwagener | 5:3dd9b0a35aee | 52 | double max(double* values) { |
nolanwagener | 5:3dd9b0a35aee | 53 | double currentMax = values[0]; |
nolanwagener | 5:3dd9b0a35aee | 54 | |
nolanwagener | 5:3dd9b0a35aee | 55 | for(int i = 1; i < NUM_OF_WEIGHTS; i++) { |
nolanwagener | 5:3dd9b0a35aee | 56 | if(values[i] > currentMax) |
nolanwagener | 5:3dd9b0a35aee | 57 | currentMax = values[i]; |
nolanwagener | 5:3dd9b0a35aee | 58 | } |
nolanwagener | 5:3dd9b0a35aee | 59 | |
nolanwagener | 5:3dd9b0a35aee | 60 | return currentMax; |
nolanwagener | 5:3dd9b0a35aee | 61 | |
nolanwagener | 5:3dd9b0a35aee | 62 | } |
nolanwagener | 5:3dd9b0a35aee | 63 | |
nolanwagener | 5:3dd9b0a35aee | 64 | int max(int* values) { |
nolanwagener | 5:3dd9b0a35aee | 65 | int currentMax = values[0]; |
nolanwagener | 5:3dd9b0a35aee | 66 | |
nolanwagener | 5:3dd9b0a35aee | 67 | for(int i = 1; i < NUM_OF_WEIGHTS; i++) { |
nolanwagener | 5:3dd9b0a35aee | 68 | if(values[i] > currentMax) |
nolanwagener | 5:3dd9b0a35aee | 69 | currentMax = values[i]; |
nolanwagener | 5:3dd9b0a35aee | 70 | } |
nolanwagener | 5:3dd9b0a35aee | 71 | |
nolanwagener | 5:3dd9b0a35aee | 72 | return currentMax; |
nolanwagener | 5:3dd9b0a35aee | 73 | |
nolanwagener | 5:3dd9b0a35aee | 74 | } |
nolanwagener | 5:3dd9b0a35aee | 75 | |
nolanwagener | 5:3dd9b0a35aee | 76 | void correctWeights(double* wrongWeights, double* properWeights) { |
nolanwagener | 5:3dd9b0a35aee | 77 | double colors[] = { (double) colorSensor.red(), (double) colorSensor.green(), (double) colorSensor.blue() }; |
nolanwagener | 5:3dd9b0a35aee | 78 | normalize(colors); |
nolanwagener | 5:3dd9b0a35aee | 79 | wrongWeights[0] -= colors[0]; //colorSensor.red(); |
nolanwagener | 5:3dd9b0a35aee | 80 | wrongWeights[1] -= colors[1]; //colorSensor.green(); |
nolanwagener | 5:3dd9b0a35aee | 81 | wrongWeights[2] -= colors[2]; //colorSensor.blue(); |
nolanwagener | 5:3dd9b0a35aee | 82 | properWeights[0] += colors[0]; //colorSensor.red(); |
nolanwagener | 5:3dd9b0a35aee | 83 | properWeights[1] += colors[1]; //colorSensor.green(); |
nolanwagener | 5:3dd9b0a35aee | 84 | properWeights[2] += colors[2]; //colorSensor.blue(); |
nolanwagener | 5:3dd9b0a35aee | 85 | |
nolanwagener | 5:3dd9b0a35aee | 86 | normalize(wrongWeights); |
nolanwagener | 5:3dd9b0a35aee | 87 | normalize(properWeights); |
nolanwagener | 5:3dd9b0a35aee | 88 | } |
nolanwagener | 5:3dd9b0a35aee | 89 | |
nolanwagener | 5:3dd9b0a35aee | 90 | void correctWeights(int* wrongWeights, int* properWeights) { |
nolanwagener | 5:3dd9b0a35aee | 91 | wrongWeights[0] -= colorSensor.red(); |
nolanwagener | 5:3dd9b0a35aee | 92 | wrongWeights[1] -= colorSensor.green(); |
nolanwagener | 5:3dd9b0a35aee | 93 | wrongWeights[2] -= colorSensor.blue(); |
nolanwagener | 5:3dd9b0a35aee | 94 | properWeights[0] += colorSensor.red(); |
nolanwagener | 5:3dd9b0a35aee | 95 | properWeights[1] += colorSensor.green(); |
nolanwagener | 5:3dd9b0a35aee | 96 | properWeights[2] += colorSensor.blue(); |
nolanwagener | 5:3dd9b0a35aee | 97 | } |
nolanwagener | 5:3dd9b0a35aee | 98 | |
nolanwagener | 5:3dd9b0a35aee | 99 | |
nolanwagener | 5:3dd9b0a35aee | 100 | void sendData(char* color, double* redWeights, double* greenWeights, double* blueWeights, double* brownWeights, double* blackWeights, double* yellowWeights, double* whiteWeights) { |
nolanwagener | 5:3dd9b0a35aee | 101 | 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]); |
nolanwagener | 5:3dd9b0a35aee | 102 | } |
nolanwagener | 5:3dd9b0a35aee | 103 | |
nolanwagener | 5:3dd9b0a35aee | 104 | void sendData(char* color, int* redWeights, int* greenWeights, int* blueWeights, int* brownWeights, int* blackWeights, int* yellowWeights, int* whiteWeights) { |
nolanwagener | 5:3dd9b0a35aee | 105 | 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]); |
nolanwagener | 5:3dd9b0a35aee | 106 | } |
nolanwagener | 5:3dd9b0a35aee | 107 | |
nolanwagener | 5:3dd9b0a35aee | 108 | void calibrate(double* weights) { |
nolanwagener | 5:3dd9b0a35aee | 109 | int redTotal = 0; |
nolanwagener | 5:3dd9b0a35aee | 110 | int greenTotal = 0; |
nolanwagener | 5:3dd9b0a35aee | 111 | int blueTotal = 0; |
nolanwagener | 5:3dd9b0a35aee | 112 | int clearTotal = 0; |
nolanwagener | 5:3dd9b0a35aee | 113 | for(int i = 0; i < NUM_OF_TESTS; i++) { |
nolanwagener | 5:3dd9b0a35aee | 114 | colorSensor.readColors(); |
nolanwagener | 5:3dd9b0a35aee | 115 | redTotal += colorSensor.red(); |
nolanwagener | 5:3dd9b0a35aee | 116 | greenTotal += colorSensor.green(); |
nolanwagener | 5:3dd9b0a35aee | 117 | blueTotal += colorSensor.blue(); |
nolanwagener | 5:3dd9b0a35aee | 118 | clearTotal += colorSensor.clear(); |
nolanwagener | 5:3dd9b0a35aee | 119 | } |
nolanwagener | 5:3dd9b0a35aee | 120 | weights[0] = (double) redTotal; |
nolanwagener | 5:3dd9b0a35aee | 121 | weights[1] = (double) greenTotal; |
nolanwagener | 5:3dd9b0a35aee | 122 | weights[2] = (double) blueTotal; |
nolanwagener | 5:3dd9b0a35aee | 123 | weights[3] = (double) clearTotal; |
nolanwagener | 5:3dd9b0a35aee | 124 | normalize(weights); |
nolanwagener | 5:3dd9b0a35aee | 125 | |
nolanwagener | 5:3dd9b0a35aee | 126 | |
nolanwagener | 5:3dd9b0a35aee | 127 | } |
TaintedTruth | 1:d481b2912c2a | 128 | |
nolanwagener | 5:3dd9b0a35aee | 129 | /* |
nolanwagener | 5:3dd9b0a35aee | 130 | void calibrate(int* redWeights, int* greenWeights, int* blueWeights, int* brownWeights, int* blackWeights, int* desiredWeights) { |
nolanwagener | 5:3dd9b0a35aee | 131 | colorSensor.readColors(); |
nolanwagener | 5:3dd9b0a35aee | 132 | int redProduct = colorSensor.dotProduct(redWeights); |
nolanwagener | 5:3dd9b0a35aee | 133 | int greenProduct = colorSensor.dotProduct(greenWeights); |
nolanwagener | 5:3dd9b0a35aee | 134 | int blueProduct = colorSensor.dotProduct(blueWeights); |
nolanwagener | 5:3dd9b0a35aee | 135 | int brownProduct = colorSensor.dotProduct(brownWeights); |
nolanwagener | 5:3dd9b0a35aee | 136 | int blackProduct = colorSensor.dotProduct(blackWeights); |
nolanwagener | 5:3dd9b0a35aee | 137 | int products[] = {redProduct, greenProduct, blueProduct, brownProduct, blackProduct}; |
nolanwagener | 5:3dd9b0a35aee | 138 | int maxProduct = max(products); |
nolanwagener | 5:3dd9b0a35aee | 139 | char* color; |
nolanwagener | 5:3dd9b0a35aee | 140 | if(colorSensor.dotProduct(desiredWeights) == maxProduct) { |
nolanwagener | 5:3dd9b0a35aee | 141 | if(redWeights == desiredWeights) { |
nolanwagener | 5:3dd9b0a35aee | 142 | color = "Red"; |
nolanwagener | 5:3dd9b0a35aee | 143 | } else if(greenWeights == desiredWeights) { |
nolanwagener | 5:3dd9b0a35aee | 144 | color = "Green"; |
nolanwagener | 5:3dd9b0a35aee | 145 | } else if(blueWeights == desiredWeights) { |
nolanwagener | 5:3dd9b0a35aee | 146 | color = "Blue"; |
nolanwagener | 5:3dd9b0a35aee | 147 | } else if(brownWeights == desiredWeights) { |
nolanwagener | 5:3dd9b0a35aee | 148 | color = "Brown"; |
nolanwagener | 5:3dd9b0a35aee | 149 | } else if(blackWeights == desiredWeights) { |
nolanwagener | 5:3dd9b0a35aee | 150 | color = "Black"; |
nolanwagener | 5:3dd9b0a35aee | 151 | } |
nolanwagener | 5:3dd9b0a35aee | 152 | } else if(redProduct == maxProduct) { |
nolanwagener | 5:3dd9b0a35aee | 153 | correctWeights(redWeights, desiredWeights); |
nolanwagener | 5:3dd9b0a35aee | 154 | color = "Red"; |
nolanwagener | 5:3dd9b0a35aee | 155 | } else if(greenProduct == maxProduct) { |
nolanwagener | 5:3dd9b0a35aee | 156 | correctWeights(greenWeights, desiredWeights); |
nolanwagener | 5:3dd9b0a35aee | 157 | color = "Green"; |
nolanwagener | 5:3dd9b0a35aee | 158 | } else if(blueProduct == maxProduct) { |
nolanwagener | 5:3dd9b0a35aee | 159 | correctWeights(blueWeights, desiredWeights); |
nolanwagener | 5:3dd9b0a35aee | 160 | color = "Blue"; |
nolanwagener | 5:3dd9b0a35aee | 161 | } else if(brownProduct == maxProduct) { |
nolanwagener | 5:3dd9b0a35aee | 162 | correctWeights(brownWeights, desiredWeights); |
nolanwagener | 5:3dd9b0a35aee | 163 | color = "Brown"; |
nolanwagener | 5:3dd9b0a35aee | 164 | } else if(blackProduct == maxProduct) { |
nolanwagener | 5:3dd9b0a35aee | 165 | correctWeights(blackWeights, desiredWeights); |
nolanwagener | 5:3dd9b0a35aee | 166 | color = "Black"; |
nolanwagener | 5:3dd9b0a35aee | 167 | } |
nolanwagener | 5:3dd9b0a35aee | 168 | //sendData(color, redWeights, greenWeights, blueWeights, brownWeights, blackWeights); |
nolanwagener | 5:3dd9b0a35aee | 169 | } */ |
nolanwagener | 5:3dd9b0a35aee | 170 | |
nolanwagener | 5:3dd9b0a35aee | 171 | void measure(double* redWeights, double* greenWeights, double* blueWeights, double* brownWeights, double* blackWeights, double* yellowWeights, double* whiteWeights) { |
nolanwagener | 5:3dd9b0a35aee | 172 | colorSensor.readColors(); |
nolanwagener | 5:3dd9b0a35aee | 173 | double redProduct = colorSensor.dotProduct(redWeights); |
nolanwagener | 5:3dd9b0a35aee | 174 | double greenProduct = colorSensor.dotProduct(greenWeights); |
nolanwagener | 5:3dd9b0a35aee | 175 | double blueProduct = colorSensor.dotProduct(blueWeights); |
nolanwagener | 5:3dd9b0a35aee | 176 | double brownProduct = colorSensor.dotProduct(brownWeights); |
nolanwagener | 5:3dd9b0a35aee | 177 | double blackProduct = colorSensor.dotProduct(blackWeights); |
nolanwagener | 5:3dd9b0a35aee | 178 | double yellowProduct = colorSensor.dotProduct(yellowWeights); |
nolanwagener | 5:3dd9b0a35aee | 179 | double whiteProduct = colorSensor.dotProduct(whiteWeights); |
nolanwagener | 5:3dd9b0a35aee | 180 | double products[] = {redProduct, greenProduct, blueProduct, brownProduct, blackProduct, yellowProduct, whiteProduct}; |
nolanwagener | 5:3dd9b0a35aee | 181 | double maxProduct = max(products); |
nolanwagener | 5:3dd9b0a35aee | 182 | char* color; |
nolanwagener | 5:3dd9b0a35aee | 183 | if(redProduct == maxProduct) { |
nolanwagener | 5:3dd9b0a35aee | 184 | color = "Red"; |
nolanwagener | 5:3dd9b0a35aee | 185 | } else if(greenProduct == maxProduct) { |
nolanwagener | 5:3dd9b0a35aee | 186 | color = "Green"; |
nolanwagener | 5:3dd9b0a35aee | 187 | } else if(blueProduct == maxProduct) { |
nolanwagener | 5:3dd9b0a35aee | 188 | color = "Blue"; |
nolanwagener | 5:3dd9b0a35aee | 189 | } else if(brownProduct == maxProduct) { |
nolanwagener | 5:3dd9b0a35aee | 190 | color = "Brown"; |
nolanwagener | 5:3dd9b0a35aee | 191 | } else if(blackProduct == maxProduct) { |
nolanwagener | 5:3dd9b0a35aee | 192 | color = "Black"; |
nolanwagener | 5:3dd9b0a35aee | 193 | } else if(yellowProduct == maxProduct) { |
nolanwagener | 5:3dd9b0a35aee | 194 | color = "Yellow"; |
nolanwagener | 5:3dd9b0a35aee | 195 | } else if(whiteProduct == maxProduct) { |
nolanwagener | 5:3dd9b0a35aee | 196 | color = "White"; |
nolanwagener | 5:3dd9b0a35aee | 197 | } |
nolanwagener | 5:3dd9b0a35aee | 198 | sendData(color, redWeights, greenWeights, blueWeights, brownWeights, blackWeights, yellowWeights, whiteWeights); |
TaintedTruth | 2:5c0159599726 | 199 | } |
TaintedTruth | 0:1eada136b388 | 200 | |
nolanwagener | 5:3dd9b0a35aee | 201 | |
nolanwagener | 5:3dd9b0a35aee | 202 | void measure(int* redWeights, int* greenWeights, int* blueWeights, int* brownWeights, int* blackWeights, int* yellowWeights, int* whiteWeights) { |
nolanwagener | 5:3dd9b0a35aee | 203 | colorSensor.readColors(); |
nolanwagener | 5:3dd9b0a35aee | 204 | int redProduct = colorSensor.dotProduct(redWeights); |
nolanwagener | 5:3dd9b0a35aee | 205 | int greenProduct = colorSensor.dotProduct(greenWeights); |
nolanwagener | 5:3dd9b0a35aee | 206 | int blueProduct = colorSensor.dotProduct(blueWeights); |
nolanwagener | 5:3dd9b0a35aee | 207 | int brownProduct = colorSensor.dotProduct(brownWeights); |
nolanwagener | 5:3dd9b0a35aee | 208 | int blackProduct = colorSensor.dotProduct(blackWeights); |
nolanwagener | 5:3dd9b0a35aee | 209 | int yellowProduct = colorSensor.dotProduct(yellowWeights); |
nolanwagener | 5:3dd9b0a35aee | 210 | int whiteProduct = colorSensor.dotProduct(whiteWeights); |
nolanwagener | 5:3dd9b0a35aee | 211 | int products[] = {redProduct, greenProduct, blueProduct, brownProduct, blackProduct, yellowProduct, whiteProduct}; |
nolanwagener | 5:3dd9b0a35aee | 212 | int maxProduct = max(products); |
nolanwagener | 5:3dd9b0a35aee | 213 | char* color; |
nolanwagener | 5:3dd9b0a35aee | 214 | if(redProduct == maxProduct) { |
nolanwagener | 5:3dd9b0a35aee | 215 | color = "Red"; |
nolanwagener | 5:3dd9b0a35aee | 216 | } else if(greenProduct == maxProduct) { |
nolanwagener | 5:3dd9b0a35aee | 217 | color = "Green"; |
nolanwagener | 5:3dd9b0a35aee | 218 | } else if(blueProduct == maxProduct) { |
nolanwagener | 5:3dd9b0a35aee | 219 | color = "Blue"; |
nolanwagener | 5:3dd9b0a35aee | 220 | } else if(brownProduct == maxProduct) { |
nolanwagener | 5:3dd9b0a35aee | 221 | color = "Brown"; |
nolanwagener | 5:3dd9b0a35aee | 222 | } else if(blackProduct == maxProduct) { |
nolanwagener | 5:3dd9b0a35aee | 223 | color = "Black"; |
nolanwagener | 5:3dd9b0a35aee | 224 | } else if(yellowProduct == maxProduct) { |
nolanwagener | 5:3dd9b0a35aee | 225 | color = "Yellow"; |
nolanwagener | 5:3dd9b0a35aee | 226 | } else if(whiteProduct == maxProduct) { |
nolanwagener | 5:3dd9b0a35aee | 227 | color = "White"; |
nolanwagener | 5:3dd9b0a35aee | 228 | } |
nolanwagener | 5:3dd9b0a35aee | 229 | sendData(color, redWeights, greenWeights, blueWeights, brownWeights, blackWeights, yellowWeights, whiteWeights); |
nolanwagener | 5:3dd9b0a35aee | 230 | } |
nolanwagener | 5:3dd9b0a35aee | 231 | |
nolanwagener | 7:10e1ec0e8179 | 232 | void setParameters() { |
nolanwagener | 7:10e1ec0e8179 | 233 | int r_cap, g_cap, b_cap, c_cap, r_int, g_int, b_int, c_int; |
nolanwagener | 7:10e1ec0e8179 | 234 | 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); |
nolanwagener | 7:10e1ec0e8179 | 235 | colorSensor.setCapacitors(r_cap, g_cap, b_cap, c_cap); |
nolanwagener | 7:10e1ec0e8179 | 236 | colorSensor.setIntegrationTimeSlot(r_int, g_int, b_int, c_int); |
nolanwagener | 7:10e1ec0e8179 | 237 | } |
nolanwagener | 7:10e1ec0e8179 | 238 | |
nolanwagener | 7:10e1ec0e8179 | 239 | void getParameters() { |
nolanwagener | 7:10e1ec0e8179 | 240 | 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()); |
nolanwagener | 7:10e1ec0e8179 | 241 | } |
nolanwagener | 7:10e1ec0e8179 | 242 | |
nolanwagener | 7:10e1ec0e8179 | 243 | void optimizeParameters() { |
nolanwagener | 7:10e1ec0e8179 | 244 | int redDes, greenDes, blueDes, clearDes; |
nolanwagener | 7:10e1ec0e8179 | 245 | bt.scanf("%d,%d,%d,%d\r", &redDes, &greenDes, &blueDes, &clearDes); |
nolanwagener | 7:10e1ec0e8179 | 246 | colorSensor.optimise(redDes, greenDes, blueDes, clearDes); |
nolanwagener | 7:10e1ec0e8179 | 247 | } |
nolanwagener | 5:3dd9b0a35aee | 248 | |
nolanwagener | 5:3dd9b0a35aee | 249 | |
TaintedTruth | 0:1eada136b388 | 250 | int main() { |
TaintedTruth | 0:1eada136b388 | 251 | |
TaintedTruth | 2:5c0159599726 | 252 | bt.baud(115200); |
nolanwagener | 5:3dd9b0a35aee | 253 | colorSensor.setIntegrationTimeSlot(0xff, 0xf0, 0xff, 0xff); |
nolanwagener | 5:3dd9b0a35aee | 254 | colorSensor.setCapacitors(12, 7, 2, 9); |
nolanwagener | 5:3dd9b0a35aee | 255 | //colorSensor.setIntegrationTimeSlot(0xff, 0x110, 0x104, 0xff); |
nolanwagener | 5:3dd9b0a35aee | 256 | //colorSensor.setCapacitors(7, 7, 7, 7); |
nolanwagener | 5:3dd9b0a35aee | 257 | |
nolanwagener | 5:3dd9b0a35aee | 258 | |
nolanwagener | 5:3dd9b0a35aee | 259 | /* |
nolanwagener | 5:3dd9b0a35aee | 260 | int redWeights[] = {100, -50, -50}; |
nolanwagener | 5:3dd9b0a35aee | 261 | int greenWeights[] = {-50, 100, -50}; |
nolanwagener | 5:3dd9b0a35aee | 262 | int blueWeights[] = {-50, -50, 100}; |
nolanwagener | 5:3dd9b0a35aee | 263 | int brownWeights[] = {1, 0, 1}; |
nolanwagener | 5:3dd9b0a35aee | 264 | int blackWeights[] = {1, 1, 1}; |
nolanwagener | 5:3dd9b0a35aee | 265 | double redWeights[] = {311.4, 233.2, 217.6}; |
nolanwagener | 5:3dd9b0a35aee | 266 | */ |
nolanwagener | 5:3dd9b0a35aee | 267 | double redWeights[] = {318.8, 242.6, 227.4, 712.4}; |
nolanwagener | 5:3dd9b0a35aee | 268 | double greenWeights[] = {206.2, 258.2, 242.6, 640.4}; |
nolanwagener | 5:3dd9b0a35aee | 269 | double blueWeights[] = {190.8, 223.4, 307.6, 630.8}; |
nolanwagener | 5:3dd9b0a35aee | 270 | double brownWeights[] = {-205.6, -215.0, -212.4, -588.0}; |
nolanwagener | 5:3dd9b0a35aee | 271 | double blackWeights[] = {175.6, 187.6, 196.8, 530.4}; |
nolanwagener | 5:3dd9b0a35aee | 272 | double yellowWeights[] = {480.8, 542.6, 315.8, 1021.2}; |
nolanwagener | 5:3dd9b0a35aee | 273 | double whiteWeights[] = {455.6, 584.8, 578.8, 1021.8}; |
nolanwagener | 5:3dd9b0a35aee | 274 | |
nolanwagener | 5:3dd9b0a35aee | 275 | |
nolanwagener | 5:3dd9b0a35aee | 276 | normalize(redWeights); normalize(greenWeights); normalize(blueWeights); normalize(brownWeights); normalize(blackWeights); normalize(yellowWeights); normalize(whiteWeights); |
nolanwagener | 5:3dd9b0a35aee | 277 | int message; |
TaintedTruth | 0:1eada136b388 | 278 | |
TaintedTruth | 0:1eada136b388 | 279 | while(1) { |
nolanwagener | 5:3dd9b0a35aee | 280 | //takeMeasurements(20); |
TaintedTruth | 1:d481b2912c2a | 281 | |
TaintedTruth | 3:b3c41c85fb1e | 282 | |
nolanwagener | 5:3dd9b0a35aee | 283 | message = bt.getc(); |
nolanwagener | 5:3dd9b0a35aee | 284 | //bt.printf("\r%d\r", message); |
TaintedTruth | 0:1eada136b388 | 285 | |
nolanwagener | 5:3dd9b0a35aee | 286 | switch(message) { |
nolanwagener | 5:3dd9b0a35aee | 287 | case RED: |
nolanwagener | 5:3dd9b0a35aee | 288 | calibrate(redWeights); |
nolanwagener | 5:3dd9b0a35aee | 289 | sendData("", redWeights, greenWeights, blueWeights, brownWeights, blackWeights, yellowWeights, whiteWeights); |
nolanwagener | 5:3dd9b0a35aee | 290 | break; |
nolanwagener | 5:3dd9b0a35aee | 291 | case GREEN: |
nolanwagener | 5:3dd9b0a35aee | 292 | calibrate(greenWeights); |
nolanwagener | 5:3dd9b0a35aee | 293 | sendData("", redWeights, greenWeights, blueWeights, brownWeights, blackWeights, yellowWeights, whiteWeights); |
nolanwagener | 5:3dd9b0a35aee | 294 | break; |
nolanwagener | 5:3dd9b0a35aee | 295 | case BLUE: |
nolanwagener | 5:3dd9b0a35aee | 296 | calibrate(blueWeights); |
nolanwagener | 5:3dd9b0a35aee | 297 | sendData("", redWeights, greenWeights, blueWeights, brownWeights, blackWeights, yellowWeights, whiteWeights); |
nolanwagener | 5:3dd9b0a35aee | 298 | break; |
nolanwagener | 5:3dd9b0a35aee | 299 | case BROWN: |
nolanwagener | 5:3dd9b0a35aee | 300 | calibrate(brownWeights); |
nolanwagener | 5:3dd9b0a35aee | 301 | sendData("", redWeights, greenWeights, blueWeights, brownWeights, blackWeights, yellowWeights, whiteWeights); |
nolanwagener | 5:3dd9b0a35aee | 302 | break; |
nolanwagener | 5:3dd9b0a35aee | 303 | case BLACK: |
nolanwagener | 5:3dd9b0a35aee | 304 | calibrate(blackWeights); |
nolanwagener | 5:3dd9b0a35aee | 305 | sendData("", redWeights, greenWeights, blueWeights, brownWeights, blackWeights, yellowWeights, whiteWeights); |
nolanwagener | 5:3dd9b0a35aee | 306 | break; |
nolanwagener | 5:3dd9b0a35aee | 307 | case YELLOW: |
nolanwagener | 5:3dd9b0a35aee | 308 | calibrate(yellowWeights); |
nolanwagener | 5:3dd9b0a35aee | 309 | sendData("", redWeights, greenWeights, blueWeights, brownWeights, blackWeights, yellowWeights, whiteWeights); |
nolanwagener | 5:3dd9b0a35aee | 310 | break; |
nolanwagener | 5:3dd9b0a35aee | 311 | case WHITE: |
nolanwagener | 5:3dd9b0a35aee | 312 | calibrate(whiteWeights); |
nolanwagener | 5:3dd9b0a35aee | 313 | sendData("", redWeights, greenWeights, blueWeights, brownWeights, blackWeights, yellowWeights, whiteWeights); |
nolanwagener | 5:3dd9b0a35aee | 314 | break; |
nolanwagener | 5:3dd9b0a35aee | 315 | case TEST: |
nolanwagener | 5:3dd9b0a35aee | 316 | measure(redWeights,greenWeights, blueWeights, brownWeights, blackWeights, yellowWeights, whiteWeights); |
nolanwagener | 5:3dd9b0a35aee | 317 | break; |
nolanwagener | 7:10e1ec0e8179 | 318 | case SET_PARAMETERS: |
nolanwagener | 7:10e1ec0e8179 | 319 | setParameters(); |
nolanwagener | 7:10e1ec0e8179 | 320 | break; |
nolanwagener | 7:10e1ec0e8179 | 321 | case GET_PARAMETERS: |
nolanwagener | 7:10e1ec0e8179 | 322 | getParameters(); |
nolanwagener | 7:10e1ec0e8179 | 323 | break; |
nolanwagener | 7:10e1ec0e8179 | 324 | case OPTIMIZE_PARAMETERS: |
nolanwagener | 7:10e1ec0e8179 | 325 | optimizeParameters(); |
nolanwagener | 7:10e1ec0e8179 | 326 | break; |
nolanwagener | 5:3dd9b0a35aee | 327 | default: |
nolanwagener | 5:3dd9b0a35aee | 328 | break; |
nolanwagener | 5:3dd9b0a35aee | 329 | } |
TaintedTruth | 0:1eada136b388 | 330 | } |
nolanwagener | 5:3dd9b0a35aee | 331 | |
TaintedTruth | 0:1eada136b388 | 332 | } |