Ruprecht Altenburger
/
mirror_actuator_stud
template for students for mirror actuator
Diff: ControllerLoop.cpp
- Revision:
- 2:c4c4cc1bff45
- Parent:
- 1:a7fc1afe0575
diff -r a7fc1afe0575 -r c4c4cc1bff45 ControllerLoop.cpp --- a/ControllerLoop.cpp Mon May 17 12:03:40 2021 +0000 +++ b/ControllerLoop.cpp Tue Oct 19 06:46:33 2021 +0000 @@ -10,7 +10,7 @@ is_initialized = false; ti.reset(); ti.start(); - data.laser_on = false; + data.laser_on = true; } // decontructor for controller loop @@ -19,9 +19,10 @@ // ---------------------------------------------------------------------------- // this is the main loop called every Ts with high priority void ControllerLoop::loop(void){ - float w01=2*3.1415927 * 8; + float w01=2*3.1415927 * 2.0f; float xy[2]; float exc = 0; + float Amp = 0.025; PID_Cntrl vel_cntrl1(0.0158,3.17,0,0,Ts,-.8,.8); PID_Cntrl vel_cntrl2(0.0158,3.17,0,0,Ts,-.8,.8); while(1) @@ -37,7 +38,7 @@ // ------------------------------------------------------------- // at very beginning: move system slowly to find the zero pulse // set "if(0)" if you like to ommit at beginning - if(0)//!is_initialized) + if(!is_initialized) { find_index(); if(index1.positionAtIndexPulse != 0 && index2.positionAtIndexPulse != 0) @@ -46,12 +47,15 @@ else { // ------------------------ do the control first - // 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 - float v_des1 = 10.0f*sinf(2.0f* 3.14159f*8.0f*ti.read()); - float v_des2 = 10.0f*cosf(2.0f* 3.14159f*8.0f*ti.read()); + //float v_des1 = exc;//10.0f*sinf(2.0f* 3.14159f*8.0f*ti.read()); + //float v_des2 = 0;//10.0f*cosf(2.0f* 3.14159f*8.0f*ti.read()); + + float Kv = 140; + float v_des1 = data.cntrl_Vphi_des[0] + Kv * (data.cntrl_phi_des[0] - data.sens_phi[0]); + float v_des2 = data.cntrl_Vphi_des[1] + Kv * (data.cntrl_phi_des[1] - data.sens_phi[1]); data.i_des[0] = vel_cntrl1(v_des1 - data.sens_Vphi[0]); data.i_des[1] = vel_cntrl2(v_des2 - data.sens_Vphi[1]); @@ -59,7 +63,8 @@ i_des1.write(i2u(data.i_des[0])); i_des2.write(i2u(data.i_des[1])); // GPA: if you want to use the GPA, uncomment and improve following line: - exc = myGPA(data.i_des[0],data.sens_Vphi[0]); + //exc = myGPA(data.i_des[0],data.sens_Vphi[0]); + exc = myGPA(v_des1,data.sens_phi[0]); // now do trafos etc @@ -75,14 +80,18 @@ { if(mk.trafo_is_on) { - 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()); + data.cntrl_xy_des[0] = 50.0f*cosf(w01*glob_ti.read()); // make a circle in xy-co-ordinates + 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] = .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()); + float ti2 = glob_ti.read(); + data.cntrl_phi_des[0] = Amp * cosf(w01 * ti2); // make some harmonic movements directly on phi1/phi2 + data.cntrl_phi_des[1] = Amp * sinf(w01 * ti2); + data.cntrl_Vphi_des[0] = -Amp * w01 * sinf(w01 * ti2); // make some harmonic movements directly on phi1/phi2 + data.cntrl_Vphi_des[1] = Amp * w01 * cosf(w01 * ti2); + } } bool dum = mk.P2X(data.sens_phi,data.est_xy); // calculate actual xy-values, uncomment this if there are timing issues @@ -124,4 +133,11 @@ float i2 = 0.2f + Kp*(50.0f - data.sens_Vphi[1]) ; i_des1.write(i2u(i1)); i_des2.write(i2u(i2)); + } + +void ControllerLoop::reset_pids(void) +{ + // reset all cntrls. + + } \ No newline at end of file