Revision 2:c4c4cc1bff45, committed 2021-10-19

Comitter:

altb2

Date:

Tue Oct 19 06:46:33 2021 +0000

Parent:

1:a7fc1afe0575

Child:

3:d672a96eeecc

Commit message:

For Infotag, laser and trafo is enabled by default!

Changed in this revision

--- 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

--- a/ControllerLoop.h	Mon May 17 12:03:40 2021 +0000
+++ b/ControllerLoop.h	Tue Oct 19 06:46:33 2021 +0000
@@ -38,7 +38,7 @@
     virtual     ~ControllerLoop();
     void start_loop(void);
     void init_controllers(void);
-    
+    void reset_pids(void);
 
 
 private:

--- a/Lib_Cntrl/PID_Cntrl.cpp	Mon May 17 12:03:40 2021 +0000
+++ b/Lib_Cntrl/PID_Cntrl.cpp	Tue Oct 19 06:46:33 2021 +0000
@@ -14,6 +14,8 @@
     // ------------------
     this->P = P;
     this->I = I;
+    this->D = D;
+    this->tau_f = tau_f;
     this->Ts = Ts;
     this->uMin = uMin;
     this->uMax = uMax;
@@ -26,6 +28,8 @@
 {
     // -----------------------
     Ipart = initValue;
+    Dpart = 0.0f;
+    e_old = 0.0f;
 }
 
 
@@ -34,8 +38,10 @@
     // the main update 
     
     Ipart += I * Ts * e;
+    Dpart = tau_f / (Ts+tau_f) * Dpart + D/(Ts+tau_f)*(e-e_old);
+    e_old = e;
     Ipart = saturate(Ipart);
-    return saturate(P * e + Ipart);
+    return saturate(P * e + Ipart + Dpart);
 }
 
 float PID_Cntrl::saturate(float x)

--- a/Lib_Cntrl/PID_Cntrl.h	Mon May 17 12:03:40 2021 +0000
+++ b/Lib_Cntrl/PID_Cntrl.h	Tue Oct 19 06:46:33 2021 +0000
@@ -23,7 +23,7 @@
 
 private:
     float P,I,D,tau_f,Ts,uMax,uMin;
-    float Ipart;
+    float Ipart,Dpart,e_old;
 
 };
 

--- a/Lib_Misc/data_structs.h	Mon May 17 12:03:40 2021 +0000
+++ b/Lib_Misc/data_structs.h	Tue Oct 19 06:46:33 2021 +0000
@@ -7,6 +7,7 @@
     float est_xy[2];
     float sens_Vphi[2];
     float cntrl_phi_des[2];
+    float cntrl_Vphi_des[2];
     float cntrl_xy_des[2];
     float i_des[2];         // desired currents
     float wMot[4];          // desired speeds (rad/s)

--- a/main.cpp	Mon May 17 12:03:40 2021 +0000
+++ b/main.cpp	Tue Oct 19 06:46:33 2021 +0000
@@ -73,6 +73,7 @@
 //******************************************************************************
 //---------- main loop -------------
 //******************************************************************************
+
 int main()
 {
     serial_port.set_baud(115200);
@@ -85,7 +86,8 @@
     i_enable = 0;       // disable current first
     counter1.reset();   // encoder reset
     counter2.reset();   // encoder reset
-    mk.set_offsets(0,0);          // individal set values for global position
+    mk.set_offsets(982,-167);          // individal set values for global position
+    mk.trafo_is_on = true;
     glob_ti.start();
     glob_ti.reset();
     loop.init_controllers();
@@ -97,7 +99,7 @@
     uart_com.send_text((char *)"Start Mirroractuator 1.2");
    /* p1.initialize(300,10,A,0,0,0);
     p2.initialize(300,10,-A,0,0,A);*/
-    laser_on = 0;
+    laser_on = true;
     //for(int wk =0;wk<5;wk++)
     while(0)
         {