Diff: main.cpp

Revision:

2:0b64504024fe

Parent:

1:314e2e3727f2

Child:

3:477db0d9895e

diff -r 314e2e3727f2 -r 0b64504024fe main.cpp
--- a/main.cpp	Tue May 07 10:22:30 2019 +0000
+++ b/main.cpp	Tue May 07 11:04:54 2019 +0000
@@ -29,7 +29,8 @@
 void updateLoop(void);   // loop for State machine (via interrupt)
 float Ts = 0.0005f;                     // sample time of main loop
 uint16_t k = 0;
-PID_Cntrl dt1(0.0f,0.0f,17.3780,0.000625f,Ts,-3.0,3.0);
+float w=0.5;
+PID_Cntrl dt1(0.0f,0.0f,17.3780/1600.0,0.000625f,Ts,-3.0,3.0);
 PID_Cntrl pi1(1.35f,13.5f,0,1.0f,Ts,-3.0,3.0);
 PID_Cntrl pidt1(-0.0019,4.72,0.4,0.005,Ts,-3.0,3.0);
 //******************************************************************************
@@ -54,13 +55,14 @@
     float i_des = 0.0f;         // default: set motor current to zero (will be overwritten)
     if(controller_active)
         {   
-            i_des = pi1(w-x -dt1(x));
+            i_des = pi1(w-x)-dt1(x);
                 }
     out.write(i2u(i_des));
     if(++k>1000)
     {
         pc.printf("x: %1.3f, i: %1.4f\r\n",x,i_des);
         k=0;
+        w=-w;
         }    
 } // END OF updateLoop(void)
  
@@ -86,6 +88,8 @@
             {
             pc.printf("Controller active\r\n");
             // reset controller here!!!        
+            pi1.reset(0.0);
+            dt1.reset(0.0);
             }
         else
             pc.printf("Controller disabled\r\n");