Diff: ControllerLoop.cpp

Revision:

7:942fd77d5e19

Parent:

6:9ebeffe446e4

Child:

8:49ac75c42da0

--- a/ControllerLoop.cpp	Thu Apr 01 13:38:54 2021 +0000
+++ b/ControllerLoop.cpp	Thu Apr 15 05:36:55 2021 +0000
@@ -3,14 +3,16 @@
 
 
 
-ControllerLoop::ControllerLoop(float Ts) : thread(osPriorityNormal,4096)
+ControllerLoop::ControllerLoop(float Ts) : thread(osPriorityNormal,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;
     }
 
 
@@ -20,7 +22,6 @@
 void ControllerLoop::loop(void){
     float A=.6;
     uint32_t k=0;
-    float Kv = 150;
     while(1)
         {
         ThisThread::flags_wait_any(threadFlag);
@@ -33,7 +34,7 @@
   //          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;
             }
-        if(0)//!is_initialized)
+        if(!is_initialized)
             {
             find_index();
             if(index1.positionAtIndexPulse != 0 && index2.positionAtIndexPulse != 0) 
@@ -41,31 +42,56 @@
             }
         else
             {
-            short c1 = counter1 - index1.positionAtIndexPulse - mc.inc_offset[0]- mc.inc_additional_offset[0];            // get counts from Encoder
-            short c2 = counter2 - index2.positionAtIndexPulse - mc.inc_offset[1]- mc.inc_additional_offset[1];            // get counts from Encoder
+            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*3.1415927/4000.0*(float)c1);
             data.sens_Vphi[0] = diff1(c1);                 // motor velocity 
             data.sens_phi[1] = uw2pi2(2*3.1415927/4000.0*(float)c2);
-            data.sens_Vphi[1] = diff2(c2);                 // motor velocity 
-            float w01=2*3.1415927 * 3;
+            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]);
+            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] ) ;
+            // ------------------------ write outputs
+            i_des1.write(i2u(data.i_des[0]));
+            i_des2.write(i2u(data.i_des[1]));
+            // now do trafos etc
+
+            float w01=2*3.1415927 * .5;
             float w02=2*3.1415927 * 1.5;
-            
+            float xy[2];
+            if(mk.external_control) // get desired values from external source (GUI)
+                {
+                if(mk.trafo_is_on)  // use desired xy values from xternal source and transform
+                                    // otherwise external source delivers phi1, phi2 values directly
+                    {
+                    bool dum = mk.X2P(data.cntrl_xy_des,data.cntrl_phi_des);
+                    }
+                }
+            else
+                {
+                if(mk.trafo_is_on)
+                    {
+                    data.cntrl_xy_des[0] = 50.0f*cosf(w01*glob_ti.read());
+                    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] = .50f*cosf(w01*glob_ti.read());
+                    data.cntrl_phi_des[1] = .50f*sinf(w01*glob_ti.read());
+                    }
+                }
+            //bool dum = mk.P2X(data.cntrl_phi_des,data.cntrl_xy_des)
             //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];
+            //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;
-    
-            float v_des1 = Kv*(data.cntrl_phi_des[0]-data.sens_phi[0]);
-            float v_des2 = Kv*(data.cntrl_phi_des[1]-data.sens_phi[1]);
-            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] ) ;
-    //float dum = (float)(++u_test%16)/16.0f;
-    //i_des1.write(i2u(.25f*sin(2*3.14f*1.0f)));
-    //i_des1.write(i2u(data.i_des[0]));
-            i_des2.write(i2u(data.i_des[1]));
+            laser_on = data.laser_on;
             }
         }
 }
@@ -78,6 +104,12 @@
     thread.start(callback(this, &ControllerLoop::loop));
     ticker.attach(callback(this, &ControllerLoop::sendSignal), Ts);
 }
+
+float ControllerLoop::pos_cntrl(float d_phi)
+{
+    return Kv*mk.mot_inc_to_rad*round(mk.mot_rad_to_inc*(d_phi));
+    }
+
 void ControllerLoop::init_controllers(void)
 {
     float Kp = 2000 * 4.89e-7/0.094f;   // XX * J/km
@@ -97,6 +129,6 @@
     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)));
-    //i_des1.write(i2u(i1));
+    i_des1.write(i2u(i1));
     i_des2.write(i2u(i2));
     }
\ No newline at end of file