Revision 7:11dd5581c763, committed 2018-04-28

Comitter:

JamieBignell

Date:

Sat Apr 28 21:34:55 2018 +0000

Parent:

6:a652deaae134

Commit message:

Hybrid Mouse Code

Changed in this revision

--- a/calculateFunc.cpp	Sun Apr 01 18:21:50 2018 +0000
+++ b/calculateFunc.cpp	Sat Apr 28 21:34:55 2018 +0000
@@ -3,16 +3,5 @@
 
 void calculateFunc()
 {
-    //USE ERROR TO CALCULATE PID VALUES
-    Prop = errorVar;
-    Inte = Inte + errorVar;
-    Dere = ((errorVar) - (previousError));
-    
-    //BECAUSE THE CURRENT ERROR IS CALCULATED IN A FUNCTION BEFORE THIS, ON THE NEXT ITERATION WE WANT TO REMEMBER THE PREVIOUS VARIABLE
-    previousError=errorVar;
-    
-    //FORMULATE OFFSET VALUE FROM ERROR, PID VALUES AND CONSTANTS
-    PIDs = (Kp*Prop)+(Kd*Dere)+(Ki*Inte);     
-    newPIDs=PIDs;
     return; 
 }
\ No newline at end of file

--- a/driveFunc.cpp	Sun Apr 01 18:21:50 2018 +0000
+++ b/driveFunc.cpp	Sat Apr 28 21:34:55 2018 +0000
@@ -7,17 +7,17 @@
     leftMotorValTemp=initialMotorspeed-newPIDs;
     rightMotorValTemp=initialMotorspeed+newPIDs;
     
-    rightMotorValTemp=53.9*exp(2.89*0.001*rightMotorValTemp); 
-    leftMotorValTemp=53.9*exp(2.89*0.001*leftMotorValTemp); 
+    //rightMotorValTemp=53.9*exp(2.89*0.001*rightMotorValTemp); 
+    //leftMotorValTemp=53.9*exp(2.89*0.001*leftMotorValTemp); 
  
     //IF THE MOTOR SPEED GOES OUTSIDE POSSIBLE BOUNDARIES, CLAMP
-    if (leftMotorValTemp>1000)
+    if (leftMotorValTemp>=1000)
     {
-        leftMotorValTemp=1000;
+        leftMotorValTemp=999;
     }    
-    if (rightMotorValTemp>1000)
+    if (rightMotorValTemp>=1000)
     {
-        rightMotorValTemp=1000;
+        rightMotorValTemp=999;
     }    
     if (leftMotorValTemp<0)
     {
@@ -27,17 +27,36 @@
     {
         rightMotorValTemp=0;
     }     
+    
+    for (int i=0;i<=10;i++)
+    {
+        if ((leftMotorValTemp>=motorLookup[i][0]) && (leftMotorValTemp<motorLookup[i+1][0]))
+        {
+            leftMotorValTemp=motorLookup[i][1]+(leftMotorValTemp-motorLookup[i][0])*((motorLookup[i+1][1]-motorLookup[i][1])/(motorLookup[i+1][0]-motorLookup[i][0]));
+            break;
+        }
+    }
+    
+    for (int i=0;i<=10;i++)
+    {
+        if ((rightMotorValTemp>=motorLookup[i][0]) && (rightMotorValTemp<motorLookup[i+1][0]))
+        {
+            rightMotorValTemp=motorLookup[i][1]+(rightMotorValTemp-motorLookup[i][0])*((motorLookup[i+1][1]-motorLookup[i][1])/(motorLookup[i+1][0]-motorLookup[i][0]));
+            break;
+        }
+    }
+    
     if (!stopMotors)
     {   
-    leftMotorVal.pulsewidth_us(leftMotorValTemp); 
-    rightMotorVal.pulsewidth_us(rightMotorValTemp);
+        leftMotorVal.pulsewidth_us(leftMotorValTemp); 
+        rightMotorVal.pulsewidth_us(rightMotorValTemp);
     }
     else
     {
-        leftMotorVal.pulsewidth_us(0); 
-        rightMotorVal.pulsewidth_us(0);
-        for(;;){}
+        //leftMotorVal.pulsewidth_us(0); 
+        //rightMotorVal.pulsewidth_us(0);
+        //for(;;){}
     } 
-  
+
     return;  
 }
\ No newline at end of file

--- a/findLineFunc.cpp	Sun Apr 01 18:21:50 2018 +0000
+++ b/findLineFunc.cpp	Sat Apr 28 21:34:55 2018 +0000
@@ -4,10 +4,10 @@
 void findLineFunc()
 {
     //TURN RIGHT UNTIL SENSOR A6, POSITION 6, BEATS THERSHOLD
-    leftMotorVal.pulsewidth_us(300); 
+    leftMotorVal.pulsewidth_us(350); 
     rightMotorVal.pulsewidth_us(0);     
     
-    while (((sensorPin[6]).read_u16()>>4) < (sensorThreshold[6]))  
+    while (((sensorPin[0]).read_u16()>>4) < (sensorThreshold[0]))  
     {
                  
     }

--- a/funcdef.h	Sun Apr 01 18:21:50 2018 +0000
+++ b/funcdef.h	Sat Apr 28 21:34:55 2018 +0000
@@ -15,9 +15,9 @@
 #define rightMotorpin D1
 #define beeperPin D10
 
-#define Kp 65
-#define Ki 0
-#define Kd 30
+#define Kp 90//80//110
+#define Ki 0 //0.004
+#define Kd 0 //32
 #define Ac 100
 
 //i=0.05 to high
@@ -27,7 +27,7 @@
 
 //
 
-#define calibrationMotorspeed 350
+#define calibrationMotorspeed 410
 
 //http://www.sengpielaudio.com/calculator-notenames.htm
 
@@ -54,14 +54,22 @@
 extern int sensorMaxValue2[8];
 
 extern int sensorThreshold[8];
+extern int sensorErrorsdiscrete[8];
 extern float sensorErrors[8];
 
+extern float linearOffset[106][2];
+extern float motorLookup[11][2];
+
 extern float Prop;
 extern float Inte;
 extern float Dere;
 
 extern float errorVar;
+extern float largestpositiveerrorvar;
+extern float largestnegativeerrorvar;
 extern float previousError;
+extern int errorVardiscrete;
+extern int previousErrordiscrete; 
 extern float PIDs;
 extern int newPIDs;
 extern int initialMotorspeed;
@@ -69,6 +77,7 @@
 extern int rightMotorValTemp;
 extern int accumulator;
 extern bool stopMotors;
+extern int errorDetectthres;
 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 //Function Definitions
 

--- a/main.cpp	Sun Apr 01 18:21:50 2018 +0000
+++ b/main.cpp	Sat Apr 28 21:34:55 2018 +0000
@@ -15,31 +15,160 @@
 int sensorMaxValue2[] = {0,0,0,0,0,0,0,0};
 
 int sensorThreshold[] = {0,0,0,0,0,0,0,0};
+int sensorErrorsdiscrete[] = {0,0,0,0,0,0,0,0};
 float sensorErrors[] = {0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
 
+float linearOffset[106][2]={
+{   6.8275  ,   -26.0300    },
+{   6.6497  ,   -25.4700    },
+{   6.4960  ,   -24.9800    },
+{   6.2632  ,   -24.4900    },
+{   5.9921  ,   -23.9900    },
+{   5.8196  ,   -23.4700    },
+{   5.7159  ,   -22.9600    },
+{   5.5201  ,   -22.4600    },
+{   5.4829  ,   -22.0000    },
+{   5.3919  ,   -21.4900    },
+{   5.2843  ,   -20.9700    },
+{   5.2686  ,   -20.4500    },
+{   5.2052  ,   -19.9900    },
+{   5.1198  ,   -19.5200    },
+{   5.0410  ,   -19.0000    },
+{   4.9476  ,   -18.4500    },
+{   4.8565  ,   -18.0300    },
+{   4.6842  ,   -17.5100    },
+{   4.5375  ,   -16.9800    },
+{   4.4060  ,   -16.4400    },
+{   4.3064  ,   -16.0200    },
+{   4.1894  ,   -15.4500    },
+{   3.9800  ,   -14.9600    },
+{   3.8762  ,   -14.5200    },
+{   3.7609  ,   -13.9800    },
+{   3.6261  ,   -13.4800    },
+{   3.5013  ,   -13.0300    },
+{   3.3357  ,   -12.4800    },
+{   3.3057  ,   -12.0000    },
+{   3.2587  ,   -11.4900    },
+{   3.2009  ,   -11.0000    },
+{   3.1255  ,   -10.4100    },
+{   3.0409  ,   -9.9600 },
+{   2.9758  ,   -9.4600 },
+{   2.8237  ,   -9.0000 },
+{   2.7329  ,   -8.4500 },
+{   2.5944  ,   -7.9500 },
+{   2.4453  ,   -7.4300 },
+{   2.1612  ,   -6.9500 },
+{   2.1333  ,   -6.5100 },
+{   1.9384  ,   -6.0200 },
+{   1.7274  ,   -5.4800 },
+{   1.4727  ,   -4.9600 },
+{   1.2838  ,   -4.4900 },
+{   1.0132  ,   -3.9800 },
+{   0.7237  ,   -3.3500 },
+{   0.6065  ,   -3.0000 },
+{   0.4716  ,   -2.4700 },
+{   0.3420  ,   -1.9700 },
+{   0.2302  ,   -1.4900 },
+{   0.1598  ,   -0.9700 },
+{   0.0632  ,   -0.4900 },
+{   -0.0137 ,   0.0000  },
+{   -0.0542 ,   0.4800  },
+{   -0.1547 ,   1.0000  },
+{   -0.1795 ,   1.4900  },
+{   -0.2608 ,   1.9900  },
+{   -0.3267 ,   2.5000  },
+{   -0.4297 ,   2.9900  },
+{   -0.5448 ,   3.5600  },
+{   -0.6416 ,   4.0900  },
+{   -0.8621 ,   4.5000  },
+{   -1.0357 ,   5.0000  },
+{   -1.2421 ,   5.4700  },
+{   -1.5076 ,   6.0300  },
+{   -1.8054 ,   6.5500  },
+{   -2.0865 ,   7.0300  },
+{   -2.2763 ,   7.4700  },
+{   -2.5454 ,   8.0000  },
+{   -2.7537 ,   8.5600  },
+{   -2.8835 ,   8.9900  },
+{   -3.0262 ,   9.5000  },
+{   -3.2092 ,   10.0000 },
+{   -3.3651 ,   10.5600 },
+{   -3.4420 ,   10.9900 },
+{   -3.5348 ,   11.5000 },
+{   -3.5697 ,   11.9900 },
+{   -3.6499 ,   12.5300 },
+{   -3.7035 ,   13.0000 },
+{   -3.7264 ,   13.4700 },
+{   -3.8294 ,   14.0100 },
+{   -3.9171 ,   14.5100 },
+{   -4.0666 ,   15.1100 },
+{   -4.1025 ,   15.4700 },
+{   -4.3145 ,   16.0300 },
+{   -4.4127 ,   16.4800 },
+{   -4.5864 ,   17.0500 },
+{   -4.6950 ,   17.4700 },
+{   -4.8164 ,   18.0300 },
+{   -4.9033 ,   18.5300 },
+{   -5.0180 ,   19.0200 },
+{   -5.1011 ,   19.5400 },
+{   -5.2195 ,   20.0200 },
+{   -5.3131 ,   20.4700 },
+{   -5.3823 ,   21.0200 },
+{   -5.4816 ,   21.5200 },
+{   -5.5606 ,   21.9800 },
+{   -5.6365 ,   22.5000 },
+{   -5.7055 ,   23.0900 },
+{   -5.7591 ,   23.4700 },
+{   -5.9464 ,   23.9900 },
+{   -6.1486 ,   24.5400 },
+{   -6.3176 ,   25.0000 },
+{   -6.4417 ,   25.5000 },
+{   -6.6916 ,   25.9800 },
+{   -6.9624 ,   26.5600 }};
+
+float motorLookup[11][2]={
+{   0.000   ,   0.000   },
+{   20.000  ,   100.000 },
+{   300.000 ,   200.000 },
+{   520.000 ,   300.000 },
+{   660.000 ,   400.000 },
+{   750.000 ,   500.000 },
+{   820.000 ,   600.000 },
+{   860.000 ,   700.000 },
+{   920.000 ,   800.000 },
+{   940.000 ,   900.000 },
+{   1000.000,   1000.000}};
+
 float Prop=0;
 float Inte=0;
 float Dere=0;
 
 float errorVar=0;
+float largestpositiveerrorvar=0;
+float largestnegativeerrorvar=0;
+int errorVardiscrete=0;
+int previousErrordiscrete=0; 
 float previousError=0; 
 float PIDs=0;
 int newPIDs=0;
 int accumulator=0;
+
 bool stopMotors=false;
 
 int leftMotorValTemp=0;
 int rightMotorValTemp=0;
 
-int initialMotorspeed = 510;
-Ticker ticker;
+int initialMotorspeed = 550;//IF THIS IS CHANGED, CHANGE IT IN TICKER
+int errorDetectthres=100;//IF THIS IS CHANGED, CHANGE IT IN BLACK ENDGE DETECTION
 
+Ticker ticker1;
+Ticker ticker2;
 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 //Main Code
 
 void tracker() {
     //IF THE ERROR IS EQUAL TO THE LAST ERROR, START A COUNTING VARIABLE AND ADD THIS TO THE NOMINAL MOTOR SPEED (IE WHEN ERROR OF 0), IF CASE NOT TRUE, DO NOTHING/RETURN TO NORMAL SPEED FOR CORNER.
-    if (abs(PIDs)<=75)
+    if (abs(PIDs)<=errorDetectthres)
     {
         accumulator++;
         LEDVal=1;        
@@ -47,27 +176,66 @@
     else
     {
         accumulator=0;
+        errorDetectthres=65;
         LEDVal=0;
     }
     
-    if (accumulator>=4)
+    if (accumulator>=14)
+    {
+        initialMotorspeed = 975;
+    }
+    else if (accumulator>=12)
+    {
+        initialMotorspeed = 975;
+        errorDetectthres=300;
+    }
+    else if (accumulator>=10)
     {
-        initialMotorspeed = 650;
+        initialMotorspeed = 950;
+    }
+    else if (accumulator>=8)
+    {
+        initialMotorspeed = 900;
+        errorDetectthres=150;
+    }
+    else if (accumulator>=6)
+    {
+        initialMotorspeed = 600;
+        errorDetectthres=125;
+    }
+    else if (accumulator>=4)
+    {
+        initialMotorspeed = 550;
     }
     else if (accumulator>=2)
     {
-        initialMotorspeed = 600;
+        initialMotorspeed = 550;
     }
     else
     {
-        initialMotorspeed = 510;
+        initialMotorspeed = 550;
     }
 }
 
+void calculations()
+{
+    //USE ERROR TO CALCULATE PID VALUES
+    Prop = errorVar;
+    Inte = Inte + errorVar;
+    Dere = ((errorVar) - (previousError));
+    
+    //BECAUSE THE CURRENT ERROR IS CALCULATED IN A FUNCTION BEFORE THIS, ON THE NEXT ITERATION WE WANT TO REMEMBER THE PREVIOUS VARIABLE
+    previousError=errorVar;
+    previousErrordiscrete=errorVardiscrete;
+    
+    //FORMULATE OFFSET VALUE FROM ERROR, PID VALUES AND CONSTANTS
+    PIDs = (Kp*Prop)+(Kd*Dere)+(Ki*Inte);     
+    newPIDs=PIDs;
+}
+
 int main() 
 {    
-       //CONFIGURE MOTORS & TURN OFF LED
-    
+    //CONFIGURE MOTORS & TURN OFF LED    
     LEDVal=0;
     leftMotorVal.period_us(1000);
     rightMotorVal.period_us(1000);
@@ -87,14 +255,15 @@
    
     //FIND LINE
     findLineFunc();    
-    
+
     //ONCE FOUND, HAND OVER TO MAIN ALGORITHM 
-    ticker.attach(&tracker, 0.1);
+    ticker1.attach(&tracker, 0.2);
+    ticker2.attach(&calculations, 0.002);
     while(1)
     {     
         senseFunc();
         CountingVar=!CountingVar;
-        calculateFunc();
+        //calculateFunc();
         driveFunc();        
     }
     

--- 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;
 }