Diff: ControllerLoop.cpp

Revision:

11:d43f8b421d6d

Parent:

10:bfacffec199a

Child:

13:1bf960928a93

Child:

15:9f32f64eee5b

--- a/ControllerLoop.cpp	Wed Apr 28 08:26:37 2021 +0000
+++ b/ControllerLoop.cpp	Wed Apr 28 12:51:02 2021 +0000
@@ -43,15 +43,19 @@
         else
             {
             // ------------------------ 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]);//-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] ) ;
+
+            // calculate desired currents here, you can do "anything" here, 
+            // if you like to refer to values e.g. from the gui or from the trafo,
+            // please use data.xxx values, they are calculated 30 lines below
+
+            data.i_des[0] = data.i_des[1] =0.0;
+        
             // ------------------------ 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()));
+            // GPA: if you want to use the GPA, uncomment and improve following line:
+            // exc = myGPA(data.i_des[0],data.sens_Vphi[0]);
+            
             // now do trafos etc
 
             if(mk.external_control) // get desired values from external source (GUI)
@@ -62,32 +66,26 @@
                     bool dum = mk.X2P(data.cntrl_xy_des,data.cntrl_phi_des);
                     }
                 }
-            else
+            else        // this is called, when desired values are calculated here internally (e.g. pathplanner)
                 {
                 if(mk.trafo_is_on)
                     {
-                    data.cntrl_xy_des[0] = 30.0f*cosf(w01*glob_ti.read());
+                    data.cntrl_xy_des[0] = 30.0f*cosf(w01*glob_ti.read());      // make a circle in xy-co-ordinates
                     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] = .250f*cosf(w01*glob_ti.read());
+                    data.cntrl_phi_des[0] = .250f*cosf(w01*glob_ti.read());     // make some harmonic movements directly on phi1/phi2
                     data.cntrl_phi_des[1] = .250f*sinf(w01*glob_ti.read());
                     }
                 }
-            //bool dum = mk.P2X(data.cntrl_phi_des,data.cntrl_xy_des)
+            bool dum = mk.P2X(data.sens_phi,data.est_xy);       // calculate actual xy-values, uncomment this if there are timing issues
             //current_path->get_x_v(glob_ti.read(),&phi_des,&v_des);
-            
-            //data.cntrl_phi_des[0] = 0.0f*A*sinf(w01*ti.read());
-            //data.cntrl_phi_des[1] = 0.0f*A*sinf(w02*ti.read());
-            //data.est_xy[0]=data.cntrl_phi_des[0]; // temporary
-            //data.est_xy[1]=data.cntrl_phi_des[1];
-            //laser_on = 1;
-            }
+            }       // else(..) 
         laser_on = data.laser_on;
         i_enable = big_button;
-        }
+        }// endof the main loop
 }
 
 void ControllerLoop::sendSignal() {
@@ -101,24 +99,24 @@
 
 float ControllerLoop::pos_cntrl(float d_phi)
 {
-    return Kv*mk.mot_inc_to_rad*round(mk.mot_rad_to_inc*(d_phi));
+   
+   // write position controller here
+   return 0.0;
     }
 
 void ControllerLoop::init_controllers(void)
 {
-    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);
+    // set values for your velocity and position controller here!
+    
+    
 }
 // find_index: move axis slowly to detect the zero-pulse
 void ControllerLoop::find_index(void)
 {
     // use a simple P-controller to get system spinning, add a constant current to overcome friction
     float Kp = 0.005;
-    float i1 = 0.1f + Kp*(50.0f - data.sens_Vphi[0]);
-    float i2 = 0.1f + Kp*(50.0f - data.sens_Vphi[1]) ;
+    float i1 = 0.2f + Kp*(50.0f - data.sens_Vphi[0]);
+    float i2 = 0.2f + Kp*(50.0f - data.sens_Vphi[1]) ;
     i_des1.write(i2u(i1));
     i_des2.write(i2u(i2));
     }
\ No newline at end of file