Diff: ControllerLoop.cpp

Revision:

1:25a2b47ca291

Parent:

0:d2e117716219

Child:

2:92c25cb669f4

--- a/ControllerLoop.cpp	Sun May 02 19:32:30 2021 +0000
+++ b/ControllerLoop.cpp	Thu Aug 05 08:27:51 2021 +0000
@@ -21,6 +21,9 @@
 void ControllerLoop::loop(void){
     float w01=2*3.1415927 * 8;
     float xy[2];
+    float exc = 0;
+    PID_Cntrl v_cntrl_1(0.0153f, 3.06,0,0,Ts,-0.8,0.8);
+    PID_Cntrl v_cntrl_2(0.0153f, 3.06,0,0,Ts,-0.8,0.8);
     while(1)
         {
         ThisThread::flags_wait_any(threadFlag);
@@ -34,7 +37,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) 
@@ -42,19 +45,33 @@
             }
         else
             {
+            //    float Kp = 0.005;
+            //    data.i_des[0] = 0.1f + Kp*(exc+50.0f - data.sens_Vphi[0]);
             // ------------------------ 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
-
-            data.i_des[0] = data.i_des[1] =0.0;
+             //float e1 = 50 - data.sens_Vphi[0];
+             //float e2 = 50 - data.sens_Vphi[1];
+             //float v_des1 = exc;
+             //float v_des2 = 0;
+             float phi1_des = 0.025f*sinf(2.0f* 3.14159f*2.0f*ti.read());
+             float phi2_des = 0.025f*cosf(2.0f* 3.14159f*2.0f*ti.read());
+             float Kv = 123;
+             float v_des1 = Kv*(phi1_des - data.sens_phi[0]);
+             float v_des2 = Kv*(phi2_des - data.sens_phi[1]);
+             data.i_des[0] = v_cntrl_1(v_des1 - data.sens_Vphi[0]);
+             data.i_des[1] = v_cntrl_2(v_des2 - data.sens_Vphi[1]);
+             
+             //data.i_des[1] =0.0;
         
             // ------------------------ write outputs
             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