Ruprecht Altenburger
/
simple_program
Simple program for introduction of mirror actuator.
Diff: ControllerLoop.cpp
- Revision:
- 11:d43f8b421d6d
- Parent:
- 10:bfacffec199a
- Child:
- 13:1bf960928a93
--- a/ControllerLoop.cpp Wed Apr 28 08:26:37 2021 +0000 +++ b/ControllerLoop.cpp Wed Apr 28 12:51:02 2021 +0000 @@ -43,15 +43,19 @@ else { // ------------------------ 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())); - 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] ) ; + + // 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 + + data.i_des[0] = data.i_des[1] =0.0; + // ------------------------ 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())); + // GPA: if you want to use the GPA, uncomment and improve following line: + // exc = myGPA(data.i_des[0],data.sens_Vphi[0]); + // now do trafos etc if(mk.external_control) // get desired values from external source (GUI) @@ -62,32 +66,26 @@ bool dum = mk.X2P(data.cntrl_xy_des,data.cntrl_phi_des); } } - else + else // this is called, when desired values are calculated here internally (e.g. pathplanner) { if(mk.trafo_is_on) { - data.cntrl_xy_des[0] = 30.0f*cosf(w01*glob_ti.read()); + 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()); bool dum = mk.X2P(data.cntrl_xy_des,data.cntrl_phi_des); } else { - data.cntrl_phi_des[0] = .250f*cosf(w01*glob_ti.read()); + 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()); } } - //bool dum = mk.P2X(data.cntrl_phi_des,data.cntrl_xy_des) + bool dum = mk.P2X(data.sens_phi,data.est_xy); // calculate actual xy-values, uncomment this if there are timing issues //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; - } + } // else(..) laser_on = data.laser_on; i_enable = big_button; - } + }// endof the main loop } void ControllerLoop::sendSignal() { @@ -101,24 +99,24 @@ float ControllerLoop::pos_cntrl(float d_phi) { - return Kv*mk.mot_inc_to_rad*round(mk.mot_rad_to_inc*(d_phi)); + + // write position controller here + return 0.0; } void ControllerLoop::init_controllers(void) { - 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); + // set values for your velocity and position controller here! + + } // 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; - float i1 = 0.1f + Kp*(50.0f - data.sens_Vphi[0]); - float i2 = 0.1f + Kp*(50.0f - data.sens_Vphi[1]) ; + float i1 = 0.2f + Kp*(50.0f - data.sens_Vphi[0]); + float i2 = 0.2f + Kp*(50.0f - data.sens_Vphi[1]) ; i_des1.write(i2u(i1)); i_des2.write(i2u(i2)); } \ No newline at end of file