Diff: ControllerLoop.cpp

Revision:

2:c4c4cc1bff45

Parent:

1:a7fc1afe0575

--- a/ControllerLoop.cpp	Mon May 17 12:03:40 2021 +0000
+++ b/ControllerLoop.cpp	Tue Oct 19 06:46:33 2021 +0000
@@ -10,7 +10,7 @@
     is_initialized = false;
     ti.reset();
     ti.start();
-    data.laser_on = false;
+    data.laser_on = true;
     }
 
 // decontructor for controller loop
@@ -19,9 +19,10 @@
 // ----------------------------------------------------------------------------
 // this is the main loop called every Ts with high priority
 void ControllerLoop::loop(void){
-    float w01=2*3.1415927 * 8;
+    float w01=2*3.1415927 * 2.0f;
     float xy[2];
     float exc = 0;
+    float Amp = 0.025;
     PID_Cntrl vel_cntrl1(0.0158,3.17,0,0,Ts,-.8,.8); 
     PID_Cntrl vel_cntrl2(0.0158,3.17,0,0,Ts,-.8,.8);
     while(1)
@@ -37,7 +38,7 @@
         // -------------------------------------------------------------
         // at very beginning: move system slowly to find the zero pulse
         // set "if(0)" if you like to ommit at beginning
-        if(0)//!is_initialized)
+        if(!is_initialized)
             {
             find_index();
             if(index1.positionAtIndexPulse != 0 && index2.positionAtIndexPulse != 0) 
@@ -46,12 +47,15 @@
         else
             {
             // ------------------------ do the control first
-
             // 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
-            float v_des1 = 10.0f*sinf(2.0f* 3.14159f*8.0f*ti.read());
-            float v_des2 = 10.0f*cosf(2.0f* 3.14159f*8.0f*ti.read());
+            //float v_des1 = exc;//10.0f*sinf(2.0f* 3.14159f*8.0f*ti.read());
+            //float v_des2 = 0;//10.0f*cosf(2.0f* 3.14159f*8.0f*ti.read());
+            
+            float Kv = 140;
+            float v_des1 = data.cntrl_Vphi_des[0] + Kv * (data.cntrl_phi_des[0] - data.sens_phi[0]);
+            float v_des2 = data.cntrl_Vphi_des[1] + Kv * (data.cntrl_phi_des[1] - data.sens_phi[1]);
             data.i_des[0] = vel_cntrl1(v_des1 - data.sens_Vphi[0]);
             data.i_des[1] = vel_cntrl2(v_des2 - data.sens_Vphi[1]);
         
@@ -59,7 +63,8 @@
             i_des1.write(i2u(data.i_des[0]));
             i_des2.write(i2u(data.i_des[1]));
             // GPA: if you want to use the GPA, uncomment and improve following line:
-            exc = myGPA(data.i_des[0],data.sens_Vphi[0]);
+            //exc = myGPA(data.i_des[0],data.sens_Vphi[0]);
+            exc = myGPA(v_des1,data.sens_phi[0]);
             
             // now do trafos etc
 
@@ -75,14 +80,18 @@
                 {
                 if(mk.trafo_is_on)
                     {
-                    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());
+                    data.cntrl_xy_des[0] = 50.0f*cosf(w01*glob_ti.read());      // make a circle in xy-co-ordinates
+                    data.cntrl_xy_des[1] = 50.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());     // make some harmonic movements directly on phi1/phi2
-                    data.cntrl_phi_des[1] = .250f*sinf(w01*glob_ti.read());
+                    float ti2 = glob_ti.read();
+                    data.cntrl_phi_des[0] = Amp * cosf(w01 * ti2);     // make some harmonic movements directly on phi1/phi2
+                    data.cntrl_phi_des[1] = Amp * sinf(w01 * ti2);
+                    data.cntrl_Vphi_des[0] = -Amp * w01 * sinf(w01 * ti2);     // make some harmonic movements directly on phi1/phi2
+                    data.cntrl_Vphi_des[1] =  Amp * w01 * cosf(w01 * ti2);
+                    
                     }
                 }
             bool dum = mk.P2X(data.sens_phi,data.est_xy);       // calculate actual xy-values, uncomment this if there are timing issues
@@ -124,4 +133,11 @@
     float i2 = 0.2f + Kp*(50.0f - data.sens_Vphi[1]) ;
     i_des1.write(i2u(i1));
     i_des2.write(i2u(i2));
+    }
+    
+void ControllerLoop::reset_pids(void)
+{
+    // reset all cntrls.
+    
+    
     }
\ No newline at end of file