Diff: main.cpp

Revision:

7:11dd5581c763

Parent:

6:a652deaae134

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