Diff: ControllerLoop.cpp

Revision:

6:9ebeffe446e4

Parent:

5:768e10f6d372

Child:

7:942fd77d5e19

--- a/ControllerLoop.cpp	Thu Feb 25 20:28:45 2021 +0000
+++ b/ControllerLoop.cpp	Thu Apr 01 13:38:54 2021 +0000
@@ -5,10 +5,12 @@
 
 ControllerLoop::ControllerLoop(float Ts) : thread(osPriorityNormal,4096)
 {
-    thread.start(callback(this, &ControllerLoop::loop));
-    ticker.attach(callback(this, &ControllerLoop::sendSignal), Ts);
+    this->Ts = Ts;
     diff1.reset(0.0f,0);
     diff2.reset(0.0f,0);
+    is_initialized = false;
+    ti.reset();
+    ti.start();
     }
 
 
@@ -16,37 +18,85 @@
 
 
 void ControllerLoop::loop(void){
-    float Kp = 1500 * 4.89e-7/0.094;   // XX * J/km
-    float Tn = .0035;
-    float Kv=50;
-    float phi_des = 0;
-    float v_des = 0;
-    PID_Cntrl v_cntrl(Kp,Tn,0,1.0,Ts,-.95,.95);
-    Unwrapper_2pi uw2pi;
-    uint8_t k = 0;
+    float A=.6;
+    uint32_t k=0;
+    float Kv = 150;
     while(1)
         {
         ThisThread::flags_wait_any(threadFlag);
         // THE LOOP ------------------------------------------------------------
-        if(++k%64==0)
+        if(++k%500==0)
             {
-                printf("%f %f\r\n",phi_des,v_des);
+//            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;
+            }
+        if(0)//!is_initialized)
+            {
+            find_index();
+            if(index1.positionAtIndexPulse != 0 && index2.positionAtIndexPulse != 0) 
+                is_initialized=true;
             }
-        short c1 = counter1;            // get counts from Encoder
-        short c2 = counter2;            // get counts from Encoder
-        float phi1 = uw2pi(2*3.1415927/4000.0*(float)c1);
-        float vel1 = diff1(c1);                 // motor velocity 
-        float phi2 = uw2pi(2*3.1415927/4000.0*(float)c2);
-        float vel2 = diff2(c2);                 // motor velocity 
-        float w0=2*3.1415927 * 3;
-        //phi_des = 1.0*sinf(w0*ti.read());
-        current_path->get_x_v(glob_ti.read(),&phi_des,&v_des);
-        float i1 = v_cntrl(Kv * (phi_des-phi2)+v_des-vel2 );
-        //float i1 = v_cntrl(v_des-vel ) + exc;
-        i_des2.write(i2pwm(-i1));
+        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
+            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;
+            float w02=2*3.1415927 * 1.5;
+            
+            //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];
+            //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]));
+            }
         }
 }
 
 void ControllerLoop::sendSignal() {
     thread.flags_set(threadFlag);
 }
+void ControllerLoop::start_loop(void)
+{
+    thread.start(callback(this, &ControllerLoop::loop));
+    ticker.attach(callback(this, &ControllerLoop::sendSignal), Ts);
+}
+void ControllerLoop::init_controllers(void)
+{
+    float Kp = 2000 * 4.89e-7/0.094f;   // XX * J/km
+    float TroV = 1.0f / (2.0f * 3.1415f * 330.0f);
+    float Tn = .005f;
+    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);
+}
+void ControllerLoop::find_index(void)
+{
+    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)));
+    //i_des1.write(i2u(i1));
+    i_des2.write(i2u(i2));
+    }
\ No newline at end of file