Diff: ControllerLoop.cpp

Revision:

8:49ac75c42da0

Parent:

7:942fd77d5e19

Child:

10:bfacffec199a

--- a/ControllerLoop.cpp	Thu Apr 15 05:36:55 2021 +0000
+++ b/ControllerLoop.cpp	Tue Apr 27 07:50:03 2021 +0000
@@ -22,6 +22,10 @@
 void ControllerLoop::loop(void){
     float A=.6;
     uint32_t k=0;
+    float exc = 0.0;
+    float w01=2*3.1415927 * 8;
+    float w02=2*3.1415927 * 1.5;
+    float xy[2];
     while(1)
         {
         ThisThread::flags_wait_any(threadFlag);
@@ -44,23 +48,22 @@
             {
             short c1 = counter1 - index1.positionAtIndexPulse - mk.inc_offset[0]- mk.inc_additional_offset[0];            // get counts from Encoder
             short c2 = counter2 - index2.positionAtIndexPulse - mk.inc_offset[1]- mk.inc_additional_offset[1];            // get counts from Encoder
-            data.sens_phi[0] = uw2pi1(2*3.1415927/4000.0*(float)c1);
+            data.sens_phi[0] = uw2pi1(2.0f*3.1415927f/4000.0f*(float)c1);
             data.sens_Vphi[0] = diff1(c1);                 // motor velocity 
-            data.sens_phi[1] = uw2pi2(2*3.1415927/4000.0*(float)c2);
+            data.sens_phi[1] = uw2pi2(2.0f*3.1415927f/4000.0f*(float)c2);
             data.sens_Vphi[1] = diff2(c2);                 // motor velocity
             // ------------------------ do the control first
             float v_des1 = pos_cntrl(data.cntrl_phi_des[0]-data.sens_phi[0]);
-            float v_des2 = pos_cntrl(data.cntrl_phi_des[1]-data.sens_phi[1]);
+            float v_des2 = pos_cntrl(data.cntrl_phi_des[1]-data.sens_phi[1]);//-150.0f*(1+.5f*cosf(w01*glob_ti.read()));
             data.i_des[0] = v_cntrl[0](v_des1 - data.sens_Vphi[0] ) ;
             data.i_des[1] = v_cntrl[1](v_des2 - data.sens_Vphi[1] ) ;
             // ------------------------ write outputs
             i_des1.write(i2u(data.i_des[0]));
             i_des2.write(i2u(data.i_des[1]));
+            //exc =myGPA(data.i_des[1],data.sens_Vphi[1]);
+            //exc = myGPA(data.i_des[1],u2i(i_act2.read()));
             // now do trafos etc
 
-            float w01=2*3.1415927 * .5;
-            float w02=2*3.1415927 * 1.5;
-            float xy[2];
             if(mk.external_control) // get desired values from external source (GUI)
                 {
                 if(mk.trafo_is_on)  // use desired xy values from xternal source and transform
@@ -73,14 +76,14 @@
                 {
                 if(mk.trafo_is_on)
                     {
-                    data.cntrl_xy_des[0] = 50.0f*cosf(w01*glob_ti.read());
-                    data.cntrl_xy_des[1] = 50.0f*sinf(w01*glob_ti.read());
+                    data.cntrl_xy_des[0] = 30.0f*cosf(w01*glob_ti.read());
+                    data.cntrl_xy_des[1] = 30.0f*sinf(w01*glob_ti.read());
                     bool dum = mk.X2P(data.cntrl_xy_des,data.cntrl_phi_des);
                     }
                 else
                     {
-                    data.cntrl_phi_des[0] = .50f*cosf(w01*glob_ti.read());
-                    data.cntrl_phi_des[1] = .50f*sinf(w01*glob_ti.read());
+                    data.cntrl_phi_des[0] = .250f*cosf(w01*glob_ti.read());
+                    data.cntrl_phi_des[1] = .250f*sinf(w01*glob_ti.read());
                     }
                 }
             //bool dum = mk.P2X(data.cntrl_phi_des,data.cntrl_xy_des)
@@ -91,8 +94,9 @@
             //data.est_xy[0]=data.cntrl_phi_des[0]; // temporary
             //data.est_xy[1]=data.cntrl_phi_des[1];
             //laser_on = 1;
-            laser_on = data.laser_on;
             }
+        laser_on = data.laser_on;
+        i_enable = big_button;
         }
 }
 
@@ -112,9 +116,9 @@
 
 void ControllerLoop::init_controllers(void)
 {
-    float Kp = 2000 * 4.89e-7/0.094f;   // XX * J/km
-    float TroV = 1.0f / (2.0f * 3.1415f * 330.0f);
-    float Tn = .005f;
+    float Kp = 0.0158;//100 * 1.7173e-06/0.03f;   // XX * J/km
+    float TroV = 1.0f / (2.0f * 3.1415f * 250.0f);
+    float Tn = .008f;
     v_cntrl[0].setCoefficients(Kp,Kp/Tn,0.0f,1.0,TroV,Ts,-.8,.8);
     v_cntrl[1].setCoefficients(Kp,Kp/Tn,0.0f,1.0,TroV,Ts,-.8,.8);
 }