Revision 3:e693c65b04de, committed 2014-08-07

Comitter:

alpesh

Date:

Thu Aug 07 07:59:00 2014 +0000

Parent:

2:d1805e7d46fb

Child:

4:2ec05810bc47

Commit message:

changes to timestep

Changed in this revision

--- a/main.cpp	Thu Aug 07 07:11:59 2014 +0000
+++ b/main.cpp	Thu Aug 07 07:59:00 2014 +0000
@@ -391,19 +391,26 @@
     RGHCosInterrupt.enable_irq();
 }
 
-double TargetSpeed=0;
+double TargetSpeed=50.0; //Millimeter per second
 double Error =0;
 double KpTerm;
+double ErrorInt;
 double ErrorDer;
-double ErrorInt;
 
 double KpGain = 0.0;
 double KiGain = 0.0;
 double KdGain = 0.0;
 
-int counter;
+int errorcounter;
 double PreviousError [10];
 
+double PwmChange=0;
+
+double pwm;
+double NewPwm;
+
+int previousTime = 0;
+
 int main()
 
 {
@@ -416,51 +423,59 @@
     
     pc.baud(115200);
     
-    Home();
+    //Home();
     
-    //while(1)
-    
-    counter = 0;
+    errorcounter = 0;
+    PreviousError[errorcounter]=0;
     
-    PreviousError[counter]=0;
-    
-    if (GetSpeed() <= TargetSpeed)
-    
+    previousTime = RunningTime.read_us();
+
+    while(1)
     {
-        Error = TargetSpeed - GetSpeedd();
+        if (GetSpeed() <= TargetSpeed)
+        {
+            int timeStep = RunningTime.read_us() - previousTime;
+            previousTime = RunningTime.read_us();
+            
+            Error = TargetSpeed - GetSpeed();
         
-        KpTerm = Error * KpGain;
+            KpTerm = Error * KpGain;
         
-        ErrorDer = (Error - PreviousError[counter])
-    }
-    
-    {        
-        SerialTransmit();
+            ErrorDer = (Error - PreviousError[errorcounter]) / timeStep;           
+        
+            ErrorInt = ErrorInt + Error * timeStep;
         
+            PwmChange = (KpTerm + KiGain * ErrorInt + KdGain * ErrorDer);
+        
+            NewPwm = pwm + PwmChange;
         
-    
-        switch(counter)
-        {
-        case 0:
-            if (position < 200)
+            if (NewPwm > 1.0) 
             {
-                SetPower(1.0);
+                pwm = 1.0;
             }
+            
+            else if (NewPwm < 0.0)
+            {
+                pwm = 0.0;
+            }
+            
             else
             {
-                counter++;
+                pwm = NewPwm;
             }
-            break;
-        case 1:
-            if (position > 10)
+            
+            SetPower(pwm);
+
+            errorcounter ++;
+        
+            if (counter > 9)
             {
-                SetPower(-1.0);
+                counter = 0;   
             }
-            else
-            {
-                counter = 0;
-            }
-            break;
+
+            PreviousError[errorcounter] = Error;
         }
+        
+        SerialTransmit();
     }
 }
\ No newline at end of file