![](/media/cache/profiles/altb_JGUSAOG.jpg.50x50_q85.jpg)
Mirror actuator for RT2 lab
Diff: ControllerLoop.cpp
- Revision:
- 8:49ac75c42da0
- Parent:
- 7:942fd77d5e19
- Child:
- 10:bfacffec199a
diff -r 942fd77d5e19 -r 49ac75c42da0 ControllerLoop.cpp --- a/ControllerLoop.cpp Thu Apr 15 05:36:55 2021 +0000 +++ b/ControllerLoop.cpp Tue Apr 27 07:50:03 2021 +0000 @@ -22,6 +22,10 @@ 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); @@ -44,23 +48,22 @@ { 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_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*3.1415927/4000.0*(float)c2); + 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]); + float v_des2 = pos_cntrl(data.cntrl_phi_des[1]-data.sens_phi[1]);//-150.0f*(1+.5f*cosf(w01*glob_ti.read())); 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])); + //exc =myGPA(data.i_des[1],data.sens_Vphi[1]); + //exc = myGPA(data.i_des[1],u2i(i_act2.read())); // 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 @@ -73,14 +76,14 @@ { 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()); + data.cntrl_xy_des[0] = 30.0f*cosf(w01*glob_ti.read()); + data.cntrl_xy_des[1] = 30.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()); + data.cntrl_phi_des[0] = .250f*cosf(w01*glob_ti.read()); + data.cntrl_phi_des[1] = .250f*sinf(w01*glob_ti.read()); } } //bool dum = mk.P2X(data.cntrl_phi_des,data.cntrl_xy_des) @@ -91,8 +94,9 @@ //data.est_xy[0]=data.cntrl_phi_des[0]; // temporary //data.est_xy[1]=data.cntrl_phi_des[1]; //laser_on = 1; - laser_on = data.laser_on; } + laser_on = data.laser_on; + i_enable = big_button; } } @@ -112,9 +116,9 @@ 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; + float Kp = 0.0158;//100 * 1.7173e-06/0.03f; // XX * J/km + float TroV = 1.0f / (2.0f * 3.1415f * 250.0f); + float Tn = .008f; 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); }