Diff: senseFunc.cpp

Revision:

7:11dd5581c763

Parent:

6:a652deaae134

--- a/senseFunc.cpp	Sun Apr 01 18:21:50 2018 +0000
+++ b/senseFunc.cpp	Sat Apr 28 21:34:55 2018 +0000
@@ -14,56 +14,204 @@
     }
 }  
 
+float interpolation(float errorVar)
+{
+    float distanceMm=0.0;
+    for (int i=1;i<=105;i++) //not looking at first values, because interpolation occurs within range of values
+    {        
+        if ((errorVar>=linearOffset[i][0]) && (errorVar<linearOffset[i-1][0]))
+        {
+            //printf("linearOffset %8.4f", linearOffset[i-1][0]);
+            //printf("linearOffset %8.4f\r\n", linearOffset[i][0]); 
+            distanceMm=linearOffset[i][1]+(errorVar-linearOffset[i][0])*((linearOffset[i-1][1]-linearOffset[i][1])/(linearOffset[i-1][0]-linearOffset[i][0]));
+        }
+    }
+    return distanceMm;
+}
+
 void senseFunc()
 {
     //READ ALL SENSORS
     float totalError = 0.0;
   
- //   for (int i=0;i<=7;i++)
-   for (int i=7;i>=0;i--)
+    //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])));
-    }
-    /*
-    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]); 
-    */
- //   cout <<" " <<  sensorErrors[0] << endl;
- //   printf("%d  ",sensorMaxValue1[0]); 
-//    printf("%d  ",sensorMaxValue2[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))
     {
-    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);
+        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
+    {    
+        //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);
+        accumulator=0;
+        errorDetectthres=100;
+        LEDVal=0;
+    }
+       
+    errorVar = -totalError;
+    errorVar=interpolation(errorVar)/-3.81;
+ 
+    ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+    for (int i=0;i<=7;i++)
+    {
+        if ((sensorValue[i])>=(sensorThreshold[i]))
+        {
+            sensorErrorsdiscrete[i] = 1;
+        }
+        else
+        {
+            sensorErrorsdiscrete[i] = 0;
+        }
+    }
+    
+    if ((sensorErrorsdiscrete[0]) == 1)
+    {
+        if ((sensorErrorsdiscrete[1]) == 1)
+        {
+            errorVardiscrete=0;
+        }
+        else if ((sensorErrorsdiscrete[2]) == 1)
+        {
+            errorVardiscrete=-1;
+        }  
+        else
+        {
+            errorVardiscrete=-0.5;
+        }
+    }       
+    else if ((sensorErrorsdiscrete[1]) == 1)
+    {
+        if ((sensorErrorsdiscrete[0]) == 1)
+        {
+            errorVardiscrete=0;
+        }
+        else if ((sensorErrorsdiscrete[3]) == 1)
+        {
+            errorVardiscrete=1;
+        }  
+        else
+        {
+            errorVardiscrete=0.5;
+        }    
+    }   
+    else if ((sensorErrorsdiscrete[2]) == 1)
+    {
+        if ((sensorErrorsdiscrete[0]) == 1)
+        {
+            errorVardiscrete=-1;
+        }
+        else if ((sensorErrorsdiscrete[4]) == 1)
+        {
+            errorVardiscrete=-2;
+        }  
+        else
+        {
+            errorVardiscrete=-1.5;
+        }    
+    }  
+    else if ((sensorErrorsdiscrete[3]) == 1)
+    {
+        if ((sensorErrorsdiscrete[1]) == 1)
+        {
+            errorVardiscrete=1;
+        }
+        else if ((sensorErrorsdiscrete[5]) == 1)
+        {
+            errorVardiscrete=2;
+        }  
+        else
+        {
+            errorVardiscrete=1.5;
+        }    
+    } 
+    else if ((sensorErrorsdiscrete[4]) == 1)
+    {
+        if ((sensorErrorsdiscrete[2]) == 1)
+        {
+            errorVardiscrete=-2;
+        }
+        else if ((sensorErrorsdiscrete[6]) == 1)
+        {
+            errorVardiscrete=-3.5;
+        }  
+        else
+        {
+            errorVardiscrete=-2.5;
+        }    
+    } 
+    else if ((sensorErrorsdiscrete[5]) == 1)
+    {
+        if ((sensorErrorsdiscrete[3]) == 1)
+        {
+            errorVardiscrete=2;
+        }
+        else if ((sensorErrorsdiscrete[7]) == 1)
+        {
+            errorVardiscrete=3.5;
+        }  
+        else
+        {
+            errorVardiscrete=2.5;
+        }    
+    } 
+    else if ((sensorErrorsdiscrete[6]) == 1)
+    {
+        if ((sensorErrorsdiscrete[4]) == 1)
+        {
+            errorVardiscrete=-3.5;
+        }         
+        else
+        {
+            //errorVardiscrete=-4;
+            errorVardiscrete=-5;
+        }    
+    }
+    else if ((sensorErrorsdiscrete[7]) == 1)
+    {
+        if ((sensorErrorsdiscrete[5]) == 1)
+        {
+            errorVardiscrete=3.5;
+        }         
+        else
+        {
+            //errorVardiscrete=4;
+            errorVardiscrete=5;
+        }    
     }
     else
     {
-        //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);
+        errorVardiscrete=previousErrordiscrete;
     }
     
-    //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))
+    if (errorVar>largestpositiveerrorvar)
+    {
+        largestpositiveerrorvar=errorVar;
+    }
+    if (errorVar<largestnegativeerrorvar)
+    {
+        largestnegativeerrorvar=errorVar;
+    }    
+    
+    if (((errorVar)<0.2) && ((errorVardiscrete)>2))
     {
-        stopMotors=true;
-    }
+        errorVar=largestpositiveerrorvar;
+    }   
+    if (((errorVar)<0.2) && ((errorVardiscrete)<-2))
+    {
+        errorVar=largestnegativeerrorvar;
+    } 
+      
+    //printf("errorVardiscrete: %d   ",errorVardiscrete);  
+    //printf("errorVar:  %f\r\n",errorVar);   
+    //wait_ms(200);
     return;
 }