Diff: main.cpp

Revision:

6:a652deaae134

Parent:

5:dfea493f7a12

Child:

7:11dd5581c763

--- a/main.cpp	Sun Mar 11 13:15:34 2018 +0000
+++ b/main.cpp	Sun Apr 01 18:21:50 2018 +0000
@@ -15,50 +15,59 @@
 int sensorMaxValue2[] = {0,0,0,0,0,0,0,0};
 
 int sensorThreshold[] = {0,0,0,0,0,0,0,0};
-int sensorErrors[] = {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};
 
-int Prop=0;
-//int Inte;//=0;
-//int Dere;//=0;
-int errorVar=0;
-int previousError=0; 
-int PIDs=0;
-int accumulator=0;
-int counter=0;
-int delayBetweenPulses_ms=0;
-int note=0;
-int initialMotorspeed = 250;
-/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-//Interupt Definition 
+float Prop=0;
+float Inte=0;
+float Dere=0;
 
-Ticker beeper; 
-void beep()
-{    
-    if (counter == 5)
-    {
-        beeperVal.pulsewidth_us(0);
-        counter=(-1*((delayBetweenPulses_ms/10)+1));
-    }
-    else if (counter==0)
-    {
-        beeperVal.period_us(note);
-        beeperVal.pulsewidth_us(note/2); 
-    }
-    counter++;   
-    return;
-}
+float errorVar=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;
 
 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 //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)
+    {
+        accumulator++;
+        LEDVal=1;        
+    }
+    else
+    {
+        accumulator=0;
+        LEDVal=0;
+    }
+    
+    if (accumulator>=4)
+    {
+        initialMotorspeed = 650;
+    }
+    else if (accumulator>=2)
+    {
+        initialMotorspeed = 600;
+    }
+    else
+    {
+        initialMotorspeed = 510;
+    }
+}
+
 int main() 
 {    
-    //CONFIGURE BEEPER    
-    note=period_F6;
-    beeperVal.period_us(note);
-    beeperVal.pulsewidth_us(0);
+       //CONFIGURE MOTORS & TURN OFF LED
     
-    //CONFIGURE MOTORS & TURN OFF LED
     LEDVal=0;
     leftMotorVal.period_us(1000);
     rightMotorVal.period_us(1000);
@@ -67,42 +76,27 @@
     
     wait_ms(1000); 
     
-    //START BEEPER 20HZ % TURN LED ON
-    LEDVal=1; 
-    counter=0;    
-    delayBetweenPulses_ms=50;    
-    beeper.attach(&beep, 0.01);
-    //beeper.detach();
-    
+    //TURN LED ON
+    LEDVal=1;    
+ 
     //BEGIN CALIBRATION
     CalibrateFunc();
-    printCalibrateValues();   
-    
+
     //TURN OFF LED AND SET PULSES TO 2 HZ
     LEDVal=0; 
-    counter=0;    
-    delayBetweenPulses_ms=500;    
-    //beeper.attach(&beep, 0.01);
-    
-    //WAIT 3 SECONDS AND DETACH BEEPER FOR MAIN RUN
-    counter=0; 
-    beeper.detach();
-    wait_ms(3000);
-    /*
+   
     //FIND LINE
-    findLineFunc();
-    counter=0;    
-    delayBetweenPulses_ms=500;    
-    beeper.attach(&beep, 0.01);
+    findLineFunc();    
     
     //ONCE FOUND, HAND OVER TO MAIN ALGORITHM 
-    
+    ticker.attach(&tracker, 0.1);
     while(1)
     {     
         senseFunc();
+        CountingVar=!CountingVar;
         calculateFunc();
         driveFunc();        
     }
-    */
+    
     return 0;
 }
\ No newline at end of file