Revision 4:2ec05810bc47, committed 2014-08-07

Comitter:

alpesh

Date:

Thu Aug 07 08:38:18 2014 +0000

Parent:

3:e693c65b04de

Child:

5:4468f8ff5ec6

Commit message:

dsd

Changed in this revision

--- a/main.cpp	Thu Aug 07 07:59:00 2014 +0000
+++ b/main.cpp	Thu Aug 07 08:38:18 2014 +0000
@@ -397,7 +397,7 @@
 double ErrorInt;
 double ErrorDer;
 
-double KpGain = 0.0;
+double KpGain = 0.002;
 double KiGain = 0.0;
 double KdGain = 0.0;
 
@@ -411,6 +411,54 @@
 
 int previousTime = 0;
 
+void Controller ()
+    {
+         if (GetSpeed() <= TargetSpeed)
+            {
+                int timeStep = RunningTime.read_us() - previousTime;
+                previousTime = RunningTime.read_us();
+            
+                Error = TargetSpeed - GetSpeed();
+        
+                KpTerm = Error * KpGain;
+        
+                //ErrorDer = (Error - PreviousError[errorcounter]) / timeStep;           
+        
+                ErrorInt = ErrorInt + Error * timeStep;
+        
+                PwmChange = (KpTerm + KiGain * ErrorInt + KdGain * ErrorDer);
+        
+                NewPwm = pwm + PwmChange;
+        
+                if (NewPwm > 1.0) 
+                {
+                    pwm = 1.0;
+                }
+            
+                else if (NewPwm < 0.0)
+                {
+                    pwm = 0.0;
+                }
+            
+                else
+                {
+                    pwm = NewPwm;
+                }
+            
+                SetPower(pwm);
+
+                errorcounter ++;
+        
+                if (counter > 9)
+                {
+                    counter = 0;   
+                }
+
+                PreviousError[errorcounter] = Error;
+            }
+    }
+
+
 int main()
 
 {
@@ -423,7 +471,8 @@
     
     pc.baud(115200);
     
-    //Home();
+    Home();
+    //Enable();
     
     errorcounter = 0;
     PreviousError[errorcounter]=0;
@@ -432,48 +481,16 @@
 
     while(1)
     {
-        if (GetSpeed() <= TargetSpeed)
+        while(position < 200)
         {
-            int timeStep = RunningTime.read_us() - previousTime;
-            previousTime = RunningTime.read_us();
-            
-            Error = TargetSpeed - GetSpeed();
-        
-            KpTerm = Error * KpGain;
-        
-            ErrorDer = (Error - PreviousError[errorcounter]) / timeStep;           
-        
-            ErrorInt = ErrorInt + Error * timeStep;
-        
-            PwmChange = (KpTerm + KiGain * ErrorInt + KdGain * ErrorDer);
-        
-            NewPwm = pwm + PwmChange;
+            Controller();
+        }
         
-            if (NewPwm > 1.0) 
-            {
-                pwm = 1.0;
-            }
-            
-            else if (NewPwm < 0.0)
-            {
-                pwm = 0.0;
-            }
-            
-            else
-            {
-                pwm = NewPwm;
-            }
-            
-            SetPower(pwm);
-
-            errorcounter ++;
+        TargetSpeed = -50.0
         
-            if (counter > 9)
-            {
-                counter = 0;   
-            }
-
-            PreviousError[errorcounter] = Error;
+        while(position > 10)
+        {
+            Controller();
         }
         
         SerialTransmit();