Ruprecht Altenburger
/
simple_program
Simple program for introduction of mirror actuator.
ControllerLoop.cpp@10:bfacffec199a, 2021-04-28 (annotated)
- Committer:
- altb2
- Date:
- Wed Apr 28 08:26:37 2021 +0000
- Revision:
- 10:bfacffec199a
- Parent:
- 8:49ac75c42da0
- Child:
- 11:d43f8b421d6d
template 1;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
altb2 | 5:768e10f6d372 | 1 | #include "ControllerLoop.h" |
altb2 | 5:768e10f6d372 | 2 | using namespace std; |
altb2 | 5:768e10f6d372 | 3 | |
altb2 | 10:bfacffec199a | 4 | // contructor for controller loop |
altb2 | 10:bfacffec199a | 5 | ControllerLoop::ControllerLoop(float Ts) : thread(osPriorityHigh,4096), dout1(PB_9) |
altb2 | 5:768e10f6d372 | 6 | { |
altb2 | 6:9ebeffe446e4 | 7 | this->Ts = Ts; |
altb2 | 5:768e10f6d372 | 8 | diff1.reset(0.0f,0); |
altb2 | 5:768e10f6d372 | 9 | diff2.reset(0.0f,0); |
altb2 | 6:9ebeffe446e4 | 10 | is_initialized = false; |
altb2 | 6:9ebeffe446e4 | 11 | ti.reset(); |
altb2 | 6:9ebeffe446e4 | 12 | ti.start(); |
altb2 | 7:942fd77d5e19 | 13 | data.laser_on = false; |
altb2 | 5:768e10f6d372 | 14 | } |
altb2 | 5:768e10f6d372 | 15 | |
altb2 | 10:bfacffec199a | 16 | // decontructor for controller loop |
altb2 | 5:768e10f6d372 | 17 | ControllerLoop::~ControllerLoop() {} |
altb2 | 5:768e10f6d372 | 18 | |
altb2 | 10:bfacffec199a | 19 | // ---------------------------------------------------------------------------- |
altb2 | 10:bfacffec199a | 20 | // this is the main loop called every Ts with high priority |
altb2 | 5:768e10f6d372 | 21 | void ControllerLoop::loop(void){ |
altb2 | 8:49ac75c42da0 | 22 | float w01=2*3.1415927 * 8; |
altb2 | 8:49ac75c42da0 | 23 | float xy[2]; |
altb2 | 5:768e10f6d372 | 24 | while(1) |
altb2 | 5:768e10f6d372 | 25 | { |
altb2 | 5:768e10f6d372 | 26 | ThisThread::flags_wait_any(threadFlag); |
altb2 | 5:768e10f6d372 | 27 | // THE LOOP ------------------------------------------------------------ |
altb2 | 10:bfacffec199a | 28 | short c1 = counter1 - index1.positionAtIndexPulse - mk.inc_offset[0]- mk.inc_additional_offset[0]; // get counts from Encoder |
altb2 | 10:bfacffec199a | 29 | short c2 = counter2 - index2.positionAtIndexPulse - mk.inc_offset[1]- mk.inc_additional_offset[1]; // get counts from Encoder |
altb2 | 10:bfacffec199a | 30 | data.sens_phi[0] = uw2pi1(2.0f*3.1415927f/4000.0f*(float)c1); |
altb2 | 10:bfacffec199a | 31 | data.sens_Vphi[0] = diff1(c1); // motor velocity |
altb2 | 10:bfacffec199a | 32 | data.sens_phi[1] = uw2pi2(2.0f*3.1415927f/4000.0f*(float)c2); |
altb2 | 10:bfacffec199a | 33 | data.sens_Vphi[1] = diff2(c2); // motor velocity |
altb2 | 10:bfacffec199a | 34 | // ------------------------------------------------------------- |
altb2 | 10:bfacffec199a | 35 | // at very beginning: move system slowly to find the zero pulse |
altb2 | 10:bfacffec199a | 36 | // set "if(0)" if you like to ommit at beginning |
altb2 | 7:942fd77d5e19 | 37 | if(!is_initialized) |
altb2 | 6:9ebeffe446e4 | 38 | { |
altb2 | 6:9ebeffe446e4 | 39 | find_index(); |
altb2 | 6:9ebeffe446e4 | 40 | if(index1.positionAtIndexPulse != 0 && index2.positionAtIndexPulse != 0) |
altb2 | 6:9ebeffe446e4 | 41 | is_initialized=true; |
altb2 | 5:768e10f6d372 | 42 | } |
altb2 | 6:9ebeffe446e4 | 43 | else |
altb2 | 6:9ebeffe446e4 | 44 | { |
altb2 | 7:942fd77d5e19 | 45 | // ------------------------ do the control first |
altb2 | 7:942fd77d5e19 | 46 | float v_des1 = pos_cntrl(data.cntrl_phi_des[0]-data.sens_phi[0]); |
altb2 | 8:49ac75c42da0 | 47 | float v_des2 = pos_cntrl(data.cntrl_phi_des[1]-data.sens_phi[1]);//-150.0f*(1+.5f*cosf(w01*glob_ti.read())); |
altb2 | 7:942fd77d5e19 | 48 | data.i_des[0] = v_cntrl[0](v_des1 - data.sens_Vphi[0] ) ; |
altb2 | 7:942fd77d5e19 | 49 | data.i_des[1] = v_cntrl[1](v_des2 - data.sens_Vphi[1] ) ; |
altb2 | 7:942fd77d5e19 | 50 | // ------------------------ write outputs |
altb2 | 7:942fd77d5e19 | 51 | i_des1.write(i2u(data.i_des[0])); |
altb2 | 7:942fd77d5e19 | 52 | i_des2.write(i2u(data.i_des[1])); |
altb2 | 8:49ac75c42da0 | 53 | //exc =myGPA(data.i_des[1],data.sens_Vphi[1]); |
altb2 | 8:49ac75c42da0 | 54 | //exc = myGPA(data.i_des[1],u2i(i_act2.read())); |
altb2 | 7:942fd77d5e19 | 55 | // now do trafos etc |
altb2 | 7:942fd77d5e19 | 56 | |
altb2 | 7:942fd77d5e19 | 57 | if(mk.external_control) // get desired values from external source (GUI) |
altb2 | 7:942fd77d5e19 | 58 | { |
altb2 | 7:942fd77d5e19 | 59 | if(mk.trafo_is_on) // use desired xy values from xternal source and transform |
altb2 | 7:942fd77d5e19 | 60 | // otherwise external source delivers phi1, phi2 values directly |
altb2 | 7:942fd77d5e19 | 61 | { |
altb2 | 7:942fd77d5e19 | 62 | bool dum = mk.X2P(data.cntrl_xy_des,data.cntrl_phi_des); |
altb2 | 7:942fd77d5e19 | 63 | } |
altb2 | 7:942fd77d5e19 | 64 | } |
altb2 | 7:942fd77d5e19 | 65 | else |
altb2 | 7:942fd77d5e19 | 66 | { |
altb2 | 7:942fd77d5e19 | 67 | if(mk.trafo_is_on) |
altb2 | 7:942fd77d5e19 | 68 | { |
altb2 | 8:49ac75c42da0 | 69 | data.cntrl_xy_des[0] = 30.0f*cosf(w01*glob_ti.read()); |
altb2 | 8:49ac75c42da0 | 70 | data.cntrl_xy_des[1] = 30.0f*sinf(w01*glob_ti.read()); |
altb2 | 7:942fd77d5e19 | 71 | bool dum = mk.X2P(data.cntrl_xy_des,data.cntrl_phi_des); |
altb2 | 7:942fd77d5e19 | 72 | } |
altb2 | 7:942fd77d5e19 | 73 | else |
altb2 | 7:942fd77d5e19 | 74 | { |
altb2 | 8:49ac75c42da0 | 75 | data.cntrl_phi_des[0] = .250f*cosf(w01*glob_ti.read()); |
altb2 | 8:49ac75c42da0 | 76 | data.cntrl_phi_des[1] = .250f*sinf(w01*glob_ti.read()); |
altb2 | 7:942fd77d5e19 | 77 | } |
altb2 | 7:942fd77d5e19 | 78 | } |
altb2 | 7:942fd77d5e19 | 79 | //bool dum = mk.P2X(data.cntrl_phi_des,data.cntrl_xy_des) |
altb2 | 6:9ebeffe446e4 | 80 | //current_path->get_x_v(glob_ti.read(),&phi_des,&v_des); |
altb2 | 6:9ebeffe446e4 | 81 | |
altb2 | 7:942fd77d5e19 | 82 | //data.cntrl_phi_des[0] = 0.0f*A*sinf(w01*ti.read()); |
altb2 | 7:942fd77d5e19 | 83 | //data.cntrl_phi_des[1] = 0.0f*A*sinf(w02*ti.read()); |
altb2 | 7:942fd77d5e19 | 84 | //data.est_xy[0]=data.cntrl_phi_des[0]; // temporary |
altb2 | 7:942fd77d5e19 | 85 | //data.est_xy[1]=data.cntrl_phi_des[1]; |
altb2 | 6:9ebeffe446e4 | 86 | //laser_on = 1; |
altb2 | 6:9ebeffe446e4 | 87 | } |
altb2 | 8:49ac75c42da0 | 88 | laser_on = data.laser_on; |
altb2 | 8:49ac75c42da0 | 89 | i_enable = big_button; |
altb2 | 5:768e10f6d372 | 90 | } |
altb2 | 5:768e10f6d372 | 91 | } |
altb2 | 5:768e10f6d372 | 92 | |
altb2 | 5:768e10f6d372 | 93 | void ControllerLoop::sendSignal() { |
altb2 | 5:768e10f6d372 | 94 | thread.flags_set(threadFlag); |
altb2 | 5:768e10f6d372 | 95 | } |
altb2 | 6:9ebeffe446e4 | 96 | void ControllerLoop::start_loop(void) |
altb2 | 6:9ebeffe446e4 | 97 | { |
altb2 | 6:9ebeffe446e4 | 98 | thread.start(callback(this, &ControllerLoop::loop)); |
altb2 | 6:9ebeffe446e4 | 99 | ticker.attach(callback(this, &ControllerLoop::sendSignal), Ts); |
altb2 | 6:9ebeffe446e4 | 100 | } |
altb2 | 7:942fd77d5e19 | 101 | |
altb2 | 7:942fd77d5e19 | 102 | float ControllerLoop::pos_cntrl(float d_phi) |
altb2 | 7:942fd77d5e19 | 103 | { |
altb2 | 7:942fd77d5e19 | 104 | return Kv*mk.mot_inc_to_rad*round(mk.mot_rad_to_inc*(d_phi)); |
altb2 | 7:942fd77d5e19 | 105 | } |
altb2 | 7:942fd77d5e19 | 106 | |
altb2 | 6:9ebeffe446e4 | 107 | void ControllerLoop::init_controllers(void) |
altb2 | 6:9ebeffe446e4 | 108 | { |
altb2 | 8:49ac75c42da0 | 109 | float Kp = 0.0158;//100 * 1.7173e-06/0.03f; // XX * J/km |
altb2 | 8:49ac75c42da0 | 110 | float TroV = 1.0f / (2.0f * 3.1415f * 250.0f); |
altb2 | 8:49ac75c42da0 | 111 | float Tn = .008f; |
altb2 | 6:9ebeffe446e4 | 112 | v_cntrl[0].setCoefficients(Kp,Kp/Tn,0.0f,1.0,TroV,Ts,-.8,.8); |
altb2 | 6:9ebeffe446e4 | 113 | v_cntrl[1].setCoefficients(Kp,Kp/Tn,0.0f,1.0,TroV,Ts,-.8,.8); |
altb2 | 6:9ebeffe446e4 | 114 | } |
altb2 | 10:bfacffec199a | 115 | // find_index: move axis slowly to detect the zero-pulse |
altb2 | 6:9ebeffe446e4 | 116 | void ControllerLoop::find_index(void) |
altb2 | 6:9ebeffe446e4 | 117 | { |
altb2 | 10:bfacffec199a | 118 | // use a simple P-controller to get system spinning, add a constant current to overcome friction |
altb2 | 6:9ebeffe446e4 | 119 | float Kp = 0.005; |
altb2 | 10:bfacffec199a | 120 | float i1 = 0.1f + Kp*(50.0f - data.sens_Vphi[0]); |
altb2 | 10:bfacffec199a | 121 | float i2 = 0.1f + Kp*(50.0f - data.sens_Vphi[1]) ; |
altb2 | 7:942fd77d5e19 | 122 | i_des1.write(i2u(i1)); |
altb2 | 6:9ebeffe446e4 | 123 | i_des2.write(i2u(i2)); |
altb2 | 6:9ebeffe446e4 | 124 | } |