Diff: ControllerLoop.cpp

Revision:

10:bfacffec199a

Parent:

8:49ac75c42da0

Child:

11:d43f8b421d6d

--- a/ControllerLoop.cpp	Tue Apr 27 07:50:50 2021 +0000
+++ b/ControllerLoop.cpp	Wed Apr 28 08:26:37 2021 +0000
@@ -1,43 +1,39 @@
 #include "ControllerLoop.h"
 using namespace std;
 
-
-
-ControllerLoop::ControllerLoop(float Ts) : thread(osPriorityNormal,4096), dout1(PB_9)
+// contructor for controller loop
+ControllerLoop::ControllerLoop(float Ts) : thread(osPriorityHigh,4096), dout1(PB_9)
 {
     this->Ts = Ts;
     diff1.reset(0.0f,0);
     diff2.reset(0.0f,0);
-    Kv = 150;
     is_initialized = false;
     ti.reset();
     ti.start();
     data.laser_on = false;
     }
 
-
+// decontructor for controller loop
 ControllerLoop::~ControllerLoop() {}
 
-
+// ----------------------------------------------------------------------------
+// this is the main loop called every Ts with high priority
 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);
         // THE LOOP ------------------------------------------------------------
-        if(++k%500==0)
-            {
-//            short c1 = counter1;            // get counts from Encoder
-//            short c2 = counter2;            // get counts from Encoder
-            
-  //          printf("1: %d %d, 2: %d %d\r\n",index1.positionAtIndexPulse,c1-index1.positionAtIndexPulse-mc.inc_offset[0],index2.positionAtIndexPulse,c2-index2.positionAtIndexPulse-mc.inc_offset[1]);
-            //led1=!led1;
-            }
+        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.0f*3.1415927f/4000.0f*(float)c1);
+        data.sens_Vphi[0] = diff1(c1);                 // motor velocity 
+        data.sens_phi[1] = uw2pi2(2.0f*3.1415927f/4000.0f*(float)c2);
+        data.sens_Vphi[1] = diff2(c2);                 // motor velocity
+        // -------------------------------------------------------------
+        // at very beginning: move system slowly to find the zero pulse
+        // set "if(0)" if you like to ommit at beginning
         if(!is_initialized)
             {
             find_index();
@@ -46,12 +42,6 @@
             }
         else
             {
-            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.0f*3.1415927f/4000.0f*(float)c1);
-            data.sens_Vphi[0] = diff1(c1);                 // motor velocity 
-            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]);//-150.0f*(1+.5f*cosf(w01*glob_ti.read()));
@@ -122,17 +112,13 @@
     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);
 }
+// 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;
-    short counts1 = counter1;            // get counts from Encoder
-    float vel1 = diff1(counts1);                 // motor velocity 
-    short counts2 = counter2;            // get counts from Encoder
-    float vel2 = diff2(counts2);                 // motor velocity 
-    float i1 = Kp*(25.0f - vel1 );
-    float i2 = Kp*(25.0f - vel2 ) ;
-    //float dum = (float)(++u_test%16)/16.0f;
-    //i_des1.write(i2u(.25f*sin(2*3.14f*1.0f)));
+    float i1 = 0.1f + Kp*(50.0f - data.sens_Vphi[0]);
+    float i2 = 0.1f + Kp*(50.0f - data.sens_Vphi[1]) ;
     i_des1.write(i2u(i1));
     i_des2.write(i2u(i2));
     }
\ No newline at end of file