Diff: senseFunc.cpp

Revision:

6:a652deaae134

Parent:

5:dfea493f7a12

Child:

7:11dd5581c763

diff -r dfea493f7a12 -r a652deaae134 senseFunc.cpp
--- a/senseFunc.cpp	Sun Mar 11 13:15:34 2018 +0000
+++ b/senseFunc.cpp	Sun Apr 01 18:21:50 2018 +0000
@@ -1,128 +1,69 @@
 #include <mbed.h>
 #include <funcdef.h>
-  
+//#include <iostream>
+#include <math.h>
+
+float fmax(float x, float y)
+{
+    if(x > y)
+    {
+        return x;
+    }else
+    {
+        return y;
+    }
+}  
+
 void senseFunc()
 {
     //READ ALL SENSORS
-    for (int i=0;i<=7;i++)
+    float totalError = 0.0;
+  
+ //   for (int i=0;i<=7;i++)
+   for (int i=7;i>=0;i--)
     {
         sensorValue[i]=((sensorPin[i]).read_u16()>>4); 
+        sensorErrors[i]=((1/(float)(sensorMaxValue[i]-sensorMinValue[i]))*sensorValue[i])+(1-(sensorMaxValue[i]/(float)(sensorMaxValue[i]-sensorMinValue[i])));
     }
-    
-    //USING THRESHOLDS, DETERMINE IF SENSOR ON LINE, THEN PRODUCE ERROR ARRAY
-    for (int i=0;i<=7;i++)
-    {
-        if ((sensorValue[i])>=(sensorThreshold[i]))
-        {
-            sensorErrors[i] = 1;
-        }
-        else
-        {
-            sensorErrors[i] = 0;
-        }
-    }
-    
-    //SENSOR CONFIG 1
     /*
-    6 4 2 0 1 3 5 7
-    0 0 0 1 1 0 0 0 /
-    0 0 1 1 1 0 0 0 /
-    0 0 1 1 0 0 0 0 /
-    0 1 1 1 0 0 0 0 /
-    0 1 1 0 0 0 0 0 /
-    1 1 1 0 0 0 0 0 /
-    1 1 0 0 0 0 0 0 /
-    1 0 0 0 0 0 0 0 /
-    
-    6 4 2 0 1 3 5 7
-    0 0 0 1 1 0 0 0 /
-    0 0 0 1 1 1 0 0 /
-    0 0 0 0 1 1 0 0 /
-    0 0 0 0 1 1 1 0 / 
-    0 0 0 0 0 1 1 0 /
-    0 0 0 0 0 1 1 1 /
-    0 0 0 0 0 0 1 1 /
-    0 0 0 0 0 0 0 1 /
+    printf("%f  ",sensorErrors[7]); 
+    printf("%f  ",sensorErrors[5]); 
+    printf("%f  ",sensorErrors[3]); 
+    printf("%f  ",sensorErrors[1]); 
+    printf("%f  ",sensorErrors[0]); 
+    printf("%f  ",sensorErrors[2]); 
+    printf("%f  ",sensorErrors[4]); 
+    printf("%f\r\n  ",sensorErrors[6]); 
     */
-    
-    //SENSOR CONFIG 2
-    /*
-    6 4 2 0 1 3 5 7
-    0 0 0 1 1 0 0 0 /
-    0 0 1 1 0 0 0 0 /
-    0 1 1 0 0 0 0 0 /
-    1 1 0 0 0 0 0 0 /
-    1 0 0 0 0 0 0 0 /
-    
-    6 4 2 0 1 3 5 7
-    0 0 0 1 1 0 0 0 /
-    0 0 0 0 1 1 0 0 /
-    0 0 0 0 0 1 1 0 /
-    0 0 0 0 0 0 1 1 /
-    0 0 0 0 0 0 0 1 /
-    */
+ //   cout <<" " <<  sensorErrors[0] << endl;
+ //   printf("%d  ",sensorMaxValue1[0]); 
+//    printf("%d  ",sensorMaxValue2[0]);
     
-    //DETERMINE STATES WHERE AN ERROR CAN DEFINITELY BE PRODUCED, IF NONE OF THESE STATES EXIST CURRENTLY, USE PREVIOUS ERROR
-    if (((sensorErrors[6]) == 0) && ((sensorErrors[4]) == 0) && ((sensorErrors[2]) == 0) && ((sensorErrors[0]) == 1) && ((sensorErrors[1]) == 1) && ((sensorErrors[3]) == 0) && ((sensorErrors[5]) == 0) && ((sensorErrors[7]) == 0))
-    {
-        errorVar=0;
-        //delayBetweenPulses_ms=500;  
-        //note=period_F6; 
-    } 
-       
-    else if (((sensorErrors[6]) == 0) && ((sensorErrors[4]) == 0) && ((sensorErrors[2]) == 1) && ((sensorErrors[0]) == 1) && ((sensorErrors[1]) == 0) && ((sensorErrors[3]) == 0) && ((sensorErrors[5]) == 0) && ((sensorErrors[7]) == 0))
-    {
-        errorVar=-1;
-        //delayBetweenPulses_ms=400;
-        //note=period_E6;  
-    }    
-    else if (((sensorErrors[6]) == 0) && ((sensorErrors[4]) == 1) && ((sensorErrors[2]) == 1) && ((sensorErrors[0]) == 0) && ((sensorErrors[1]) == 0) && ((sensorErrors[3]) == 0) && ((sensorErrors[5]) == 0) && ((sensorErrors[7]) == 0))
-    {
-        errorVar=-2;
-        //delayBetweenPulses_ms=300;
-        //note=period_D6;   
-    }   
-    else if (((sensorErrors[6]) == 1) && ((sensorErrors[4]) == 1) && ((sensorErrors[2]) == 0) && ((sensorErrors[0]) == 0) && ((sensorErrors[1]) == 0) && ((sensorErrors[3]) == 0) && ((sensorErrors[5]) == 0) && ((sensorErrors[7]) == 0))
-    {
-        errorVar=-3;
-        //delayBetweenPulses_ms=200; 
-        //note=period_B6;  
-    }
-    else if (((sensorErrors[6]) == 1) && ((sensorErrors[4]) == 0) && ((sensorErrors[2]) == 0) && ((sensorErrors[0]) == 0) && ((sensorErrors[1]) == 0) && ((sensorErrors[3]) == 0) && ((sensorErrors[5]) == 0) && ((sensorErrors[7]) == 0))
+ //   printf("%d  ",sensorMaxValue[0]); 
+//    printf("%d  ",sensorMinValue[0]);
+//    printf("%d  ",sensorValue[0]);
+//    printf("%f\r\n  ",sensorErrors[0]);   
+ //   printf("%6.2f\r\n  ",fmax(sensorErrors[0], 0.0)*1+fmax(sensorErrors[2],0.0)*2+fmax(sensorErrors[4],0.0)*3+fmax(sensorErrors[6], 0.0)*4);
+    if ((sensorErrors[6]<=(float)0.7) && (sensorErrors[7]<=(float)0.7))
     {
-        errorVar=-4;
-        //delayBetweenPulses_ms=100;
-        //note=period_A6;   
-    }
-    
-    else if (((sensorErrors[6]) == 0) && ((sensorErrors[4]) == 0) && ((sensorErrors[2]) == 0) && ((sensorErrors[0]) == 0) && ((sensorErrors[1]) == 1) && ((sensorErrors[3]) == 1) && ((sensorErrors[5]) == 0) && ((sensorErrors[7]) == 0))
-    {
-        errorVar=1;
-        //delayBetweenPulses_ms=400;
-        //note=period_E6;   
-    }
-    else if (((sensorErrors[6]) == 0) && ((sensorErrors[4]) == 0) && ((sensorErrors[2]) == 0) && ((sensorErrors[0]) == 0) && ((sensorErrors[1]) == 0) && ((sensorErrors[3]) == 1) && ((sensorErrors[5]) == 1) && ((sensorErrors[7]) == 0))
-    {
-        errorVar=2;
-        //delayBetweenPulses_ms=300; 
-        //note=period_D6;  
-    }    
-    else if (((sensorErrors[6]) == 0) && ((sensorErrors[4]) == 0) && ((sensorErrors[2]) == 0) && ((sensorErrors[0]) == 0) && ((sensorErrors[1]) == 0) && ((sensorErrors[3]) == 0) && ((sensorErrors[5]) == 1) && ((sensorErrors[7]) == 1))
-    {
-        errorVar=3;
-        //delayBetweenPulses_ms=200;
-        //note=period_B6;   
-    }
-    else if (((sensorErrors[6]) == 0) && ((sensorErrors[4]) == 0) && ((sensorErrors[2]) == 0) && ((sensorErrors[0]) == 0) && ((sensorErrors[1]) == 0) && ((sensorErrors[3]) == 0) && ((sensorErrors[5]) == 0) && ((sensorErrors[7]) == 1))
-    {
-        errorVar=4;
-        //delayBetweenPulses_ms=100; 
-        //note=period_A6;  
+    totalError = (fmax(sensorErrors[0], 0.0)*1+fmax(sensorErrors[2],0.0)*2+fmax(sensorErrors[4],0.0)*3+fmax(sensorErrors[6], 0.0)*5) - 
+    (fmax(sensorErrors[1], 0.0)*1+fmax(sensorErrors[3],0.0)*2+fmax(sensorErrors[5],0.0)*3+fmax(sensorErrors[7], 0.0)*5);
     }
     else
     {
-        errorVar=previousError;
+        //To compensate for the black lines on the ramp
+    totalError = (fmax(sensorErrors[0], 0.0)*1+fmax(sensorErrors[2],0.0)*2) - 
+    (fmax(sensorErrors[1], 0.0)*1+fmax(sensorErrors[3],0.0)*2);
     }
-        
+    
+    //printf("%6.3f\t%6.3f\t%6.3f\t%6.3f\r\n", sensorErrors[0],sensorErrors[2],sensorErrors[4],sensorErrors[6]);
+    //printf("Total error %6.3f\r\n", totalError);
+    errorVar = -totalError;
+     //pc.printf("$%d %d %d;", (int)(100*(sensorErrors[0])),(int)(100*(sensorErrors[2])),(int)(100*(sensorErrors[4])));
+    //wait_ms(1);
+    if((sensorErrors[0] < 0.1f) && (sensorErrors[1] < 0.1f) && (sensorErrors[2] < 0.1f) && (sensorErrors[3] < 0.1) && (sensorErrors[4] < 0.1f) && (sensorErrors[5] < 0.1f) && (sensorErrors[6] < 0.1) && (sensorErrors[7] < 0.1f))
+    {
+        stopMotors=true;
+    }
     return;
-}
\ No newline at end of file
+}