Diff: edge_sensor/edge_color.cpp

Revision:

5:eba500888787

Parent:

0:f0de320e23ac

Child:

6:44ca704f2bc1

--- a/edge_sensor/edge_color.cpp	Tue Dec 05 00:18:45 2017 +0000
+++ b/edge_sensor/edge_color.cpp	Tue Dec 05 00:46:52 2017 +0000
@@ -167,13 +167,19 @@
     double      numerator[3] ;
     double      a,b,c,d,e,f,g,h,i ;
     uint16_t    v[3], tmp[3] ;
-    uint16_t    L[3][3] ;
+//    uint16_t    L[3][3] ;
+    double    L[3][3] ;
+    double      ftarget[3] ;
     int         idx ;
     
+    ftarget[0] = target[0] ;
+    ftarget[1] = target[1] ;
+    ftarget[2] = target[2] ;
     printf("=== Calibrating Color Sensor %d ===\n", _id) ;
     for (idx = 0 ; idx < 3 ; idx++ ) {
         tmp[0] = tmp[1] = tmp[2] = 0 ;
         tmp[idx] = _probe ;
+
         setLEDs(tmp) ;
         wait_ms(led_interval) ;
         getAveColor(v, num_ave) ;
@@ -188,7 +194,7 @@
     printf("=== Initial Equation ===\n") ;
     for (idx = 0 ; idx < 3 ; idx++) {
         printf("%5d * R / %d + %5d * G / %d + %5d * B / %d = %d,\n",
-        L[0][idx], _probe, L[1][idx], _probe,  L[2][idx], _probe, target[idx]) ;
+        (int)L[0][idx], _probe, (int)L[1][idx], _probe,  (int)L[2][idx], _probe, target[idx]) ;
     }
 
      a = L[0][0] ; b = L[1][0] ; c = L[2][0] ;
@@ -198,24 +204,24 @@
     denominator = a * (f * h - e * i) + b * (d * i - f * g) + c * (e * g - d * h) ;
 
     if (denominator != 0) {
-        numerator[0] = (f * h - e * i) * target[0] 
-            + b * (i * target[1] - f * target[2]) 
-            + c * (e * target[2] - h * target[1]) ;
+        numerator[0] = (f * h - e * i) * ftarget[0] 
+            + b * (i * ftarget[1] - f * ftarget[2]) 
+            + c * (e * ftarget[2] - h * ftarget[1]) ;
 
-        numerator[1] = -((f * g - d * i) * target[0]
-            + a * (i * target[1] - f * target[2]) 
-            + c * (d * target[2] - g * target[1])) ;
+        numerator[1] = -((f * g - d * i) * ftarget[0]
+            + a * (i * ftarget[1] - f * ftarget[2]) 
+            + c * (d * ftarget[2] - g * ftarget[1])) ;
 
-        numerator[2] = (e * g - d * h) * target[0]
-            + a * (h * target[1] - e * target[2]) 
-            + b * (d * target[2] - g * target[1]) ;
+        numerator[2] = (e * g - d * h) * ftarget[0]
+            + a * (h * ftarget[1] - e * ftarget[2]) 
+            + b * (d * ftarget[2] - g * ftarget[1]) ;
 
         for (idx = 0 ; idx < 3 ; idx++ ) {
-            _pwm[idx] = (uint16_t) ((_probe * numerator[idx]) / denominator) ;
+            _pwm[idx] = (uint16_t) (0.5 + ((double)_probe * numerator[idx]) / denominator) ;
             result[idx] = _pwm[idx] ;
         }
 
-        printf("R = %d [0x%04x] ", result[0], result[0]) ; 
+        printf("PWM R = %d [0x%04x] ", result[0], result[0]) ; 
         printf("G = %d [0x%04x] ", result[1], result[1]) ;
         printf("B = %d [0x%04x] ", result[2], result[2]) ;
         printf("\n") ;
@@ -224,6 +230,7 @@
         wait_ms(led_interval) ;
         getAveColor(v, num_ave) ;
         printf("R:%d, G:%d, B:%d\n", v[0], v[1], v[2]) ;
+        printf("============\n") ;
     } else {
         printf("calibration failed, pwm values were not updated\n") ;
     }