Ruprecht Altenburger
/
mirror_actuator_stud
template for students for mirror actuator
ControllerLoop.cpp@3:d672a96eeecc, 2022-03-06 (annotated)
- Committer:
- altb2
- Date:
- Sun Mar 06 08:57:28 2022 +0000
- Revision:
- 3:d672a96eeecc
- Parent:
- 2:c4c4cc1bff45
MirrorActuator Stud FS21
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
altb2 | 0:d2e117716219 | 1 | #include "ControllerLoop.h" |
altb2 | 0:d2e117716219 | 2 | using namespace std; |
altb2 | 0:d2e117716219 | 3 | |
altb2 | 0:d2e117716219 | 4 | // contructor for controller loop |
altb2 | 0:d2e117716219 | 5 | ControllerLoop::ControllerLoop(float Ts) : thread(osPriorityHigh,4096), dout1(PB_9) |
altb2 | 0:d2e117716219 | 6 | { |
altb2 | 0:d2e117716219 | 7 | this->Ts = Ts; |
altb2 | 0:d2e117716219 | 8 | diff1.reset(0.0f,0); |
altb2 | 0:d2e117716219 | 9 | diff2.reset(0.0f,0); |
altb2 | 0:d2e117716219 | 10 | is_initialized = false; |
altb2 | 0:d2e117716219 | 11 | ti.reset(); |
altb2 | 0:d2e117716219 | 12 | ti.start(); |
altb2 | 2:c4c4cc1bff45 | 13 | data.laser_on = true; |
altb2 | 0:d2e117716219 | 14 | } |
altb2 | 0:d2e117716219 | 15 | |
altb2 | 0:d2e117716219 | 16 | // decontructor for controller loop |
altb2 | 0:d2e117716219 | 17 | ControllerLoop::~ControllerLoop() {} |
altb2 | 0:d2e117716219 | 18 | |
altb2 | 0:d2e117716219 | 19 | // ---------------------------------------------------------------------------- |
altb2 | 0:d2e117716219 | 20 | // this is the main loop called every Ts with high priority |
altb2 | 0:d2e117716219 | 21 | void ControllerLoop::loop(void){ |
altb2 | 2:c4c4cc1bff45 | 22 | float w01=2*3.1415927 * 2.0f; |
altb2 | 0:d2e117716219 | 23 | float xy[2]; |
altb2 | 1:a7fc1afe0575 | 24 | float exc = 0; |
altb2 | 2:c4c4cc1bff45 | 25 | float Amp = 0.025; |
altb2 | 1:a7fc1afe0575 | 26 | PID_Cntrl vel_cntrl1(0.0158,3.17,0,0,Ts,-.8,.8); |
altb2 | 1:a7fc1afe0575 | 27 | PID_Cntrl vel_cntrl2(0.0158,3.17,0,0,Ts,-.8,.8); |
altb2 | 0:d2e117716219 | 28 | while(1) |
altb2 | 0:d2e117716219 | 29 | { |
altb2 | 0:d2e117716219 | 30 | ThisThread::flags_wait_any(threadFlag); |
altb2 | 0:d2e117716219 | 31 | // THE LOOP ------------------------------------------------------------ |
altb2 | 0:d2e117716219 | 32 | short c1 = counter1 - index1.positionAtIndexPulse - mk.inc_offset[0]- mk.inc_additional_offset[0]; // get counts from Encoder |
altb2 | 0:d2e117716219 | 33 | short c2 = counter2 - index2.positionAtIndexPulse - mk.inc_offset[1]- mk.inc_additional_offset[1]; // get counts from Encoder |
altb2 | 0:d2e117716219 | 34 | data.sens_phi[0] = uw2pi1(2.0f*3.1415927f/4000.0f*(float)c1); |
altb2 | 0:d2e117716219 | 35 | data.sens_Vphi[0] = diff1(c1); // motor velocity |
altb2 | 0:d2e117716219 | 36 | data.sens_phi[1] = uw2pi2(2.0f*3.1415927f/4000.0f*(float)c2); |
altb2 | 0:d2e117716219 | 37 | data.sens_Vphi[1] = diff2(c2); // motor velocity |
altb2 | 0:d2e117716219 | 38 | // ------------------------------------------------------------- |
altb2 | 0:d2e117716219 | 39 | // at very beginning: move system slowly to find the zero pulse |
altb2 | 0:d2e117716219 | 40 | // set "if(0)" if you like to ommit at beginning |
altb2 | 2:c4c4cc1bff45 | 41 | if(!is_initialized) |
altb2 | 0:d2e117716219 | 42 | { |
altb2 | 0:d2e117716219 | 43 | find_index(); |
altb2 | 0:d2e117716219 | 44 | if(index1.positionAtIndexPulse != 0 && index2.positionAtIndexPulse != 0) |
altb2 | 0:d2e117716219 | 45 | is_initialized=true; |
altb2 | 0:d2e117716219 | 46 | } |
altb2 | 0:d2e117716219 | 47 | else |
altb2 | 0:d2e117716219 | 48 | { |
altb2 | 0:d2e117716219 | 49 | // ------------------------ do the control first |
altb2 | 0:d2e117716219 | 50 | // calculate desired currents here, you can do "anything" here, |
altb2 | 0:d2e117716219 | 51 | // if you like to refer to values e.g. from the gui or from the trafo, |
altb2 | 0:d2e117716219 | 52 | // please use data.xxx values, they are calculated 30 lines below |
altb2 | 2:c4c4cc1bff45 | 53 | //float v_des1 = exc;//10.0f*sinf(2.0f* 3.14159f*8.0f*ti.read()); |
altb2 | 2:c4c4cc1bff45 | 54 | //float v_des2 = 0;//10.0f*cosf(2.0f* 3.14159f*8.0f*ti.read()); |
altb2 | 2:c4c4cc1bff45 | 55 | |
altb2 | 2:c4c4cc1bff45 | 56 | float Kv = 140; |
altb2 | 2:c4c4cc1bff45 | 57 | float v_des1 = data.cntrl_Vphi_des[0] + Kv * (data.cntrl_phi_des[0] - data.sens_phi[0]); |
altb2 | 2:c4c4cc1bff45 | 58 | float v_des2 = data.cntrl_Vphi_des[1] + Kv * (data.cntrl_phi_des[1] - data.sens_phi[1]); |
altb2 | 1:a7fc1afe0575 | 59 | data.i_des[0] = vel_cntrl1(v_des1 - data.sens_Vphi[0]); |
altb2 | 1:a7fc1afe0575 | 60 | data.i_des[1] = vel_cntrl2(v_des2 - data.sens_Vphi[1]); |
altb2 | 0:d2e117716219 | 61 | |
altb2 | 0:d2e117716219 | 62 | // ------------------------ write outputs |
altb2 | 0:d2e117716219 | 63 | i_des1.write(i2u(data.i_des[0])); |
altb2 | 0:d2e117716219 | 64 | i_des2.write(i2u(data.i_des[1])); |
altb2 | 0:d2e117716219 | 65 | // GPA: if you want to use the GPA, uncomment and improve following line: |
altb2 | 2:c4c4cc1bff45 | 66 | //exc = myGPA(data.i_des[0],data.sens_Vphi[0]); |
altb2 | 2:c4c4cc1bff45 | 67 | exc = myGPA(v_des1,data.sens_phi[0]); |
altb2 | 0:d2e117716219 | 68 | |
altb2 | 0:d2e117716219 | 69 | // now do trafos etc |
altb2 | 0:d2e117716219 | 70 | |
altb2 | 0:d2e117716219 | 71 | if(mk.external_control) // get desired values from external source (GUI) |
altb2 | 0:d2e117716219 | 72 | { |
altb2 | 0:d2e117716219 | 73 | if(mk.trafo_is_on) // use desired xy values from xternal source and transform |
altb2 | 0:d2e117716219 | 74 | // otherwise external source delivers phi1, phi2 values directly |
altb2 | 0:d2e117716219 | 75 | { |
altb2 | 0:d2e117716219 | 76 | bool dum = mk.X2P(data.cntrl_xy_des,data.cntrl_phi_des); |
altb2 | 0:d2e117716219 | 77 | } |
altb2 | 0:d2e117716219 | 78 | } |
altb2 | 0:d2e117716219 | 79 | else // this is called, when desired values are calculated here internally (e.g. pathplanner) |
altb2 | 0:d2e117716219 | 80 | { |
altb2 | 0:d2e117716219 | 81 | if(mk.trafo_is_on) |
altb2 | 0:d2e117716219 | 82 | { |
altb2 | 2:c4c4cc1bff45 | 83 | data.cntrl_xy_des[0] = 50.0f*cosf(w01*glob_ti.read()); // make a circle in xy-co-ordinates |
altb2 | 2:c4c4cc1bff45 | 84 | data.cntrl_xy_des[1] = 50.0f*sinf(w01*glob_ti.read()); |
altb2 | 0:d2e117716219 | 85 | bool dum = mk.X2P(data.cntrl_xy_des,data.cntrl_phi_des); |
altb2 | 0:d2e117716219 | 86 | } |
altb2 | 0:d2e117716219 | 87 | else |
altb2 | 0:d2e117716219 | 88 | { |
altb2 | 2:c4c4cc1bff45 | 89 | float ti2 = glob_ti.read(); |
altb2 | 2:c4c4cc1bff45 | 90 | data.cntrl_phi_des[0] = Amp * cosf(w01 * ti2); // make some harmonic movements directly on phi1/phi2 |
altb2 | 2:c4c4cc1bff45 | 91 | data.cntrl_phi_des[1] = Amp * sinf(w01 * ti2); |
altb2 | 2:c4c4cc1bff45 | 92 | data.cntrl_Vphi_des[0] = -Amp * w01 * sinf(w01 * ti2); // make some harmonic movements directly on phi1/phi2 |
altb2 | 2:c4c4cc1bff45 | 93 | data.cntrl_Vphi_des[1] = Amp * w01 * cosf(w01 * ti2); |
altb2 | 2:c4c4cc1bff45 | 94 | |
altb2 | 0:d2e117716219 | 95 | } |
altb2 | 0:d2e117716219 | 96 | } |
altb2 | 0:d2e117716219 | 97 | bool dum = mk.P2X(data.sens_phi,data.est_xy); // calculate actual xy-values, uncomment this if there are timing issues |
altb2 | 0:d2e117716219 | 98 | //current_path->get_x_v(glob_ti.read(),&phi_des,&v_des); |
altb2 | 0:d2e117716219 | 99 | } // else(..) |
altb2 | 0:d2e117716219 | 100 | laser_on = data.laser_on; |
altb2 | 0:d2e117716219 | 101 | i_enable = big_button; |
altb2 | 0:d2e117716219 | 102 | }// endof the main loop |
altb2 | 0:d2e117716219 | 103 | } |
altb2 | 0:d2e117716219 | 104 | |
altb2 | 0:d2e117716219 | 105 | void ControllerLoop::sendSignal() { |
altb2 | 0:d2e117716219 | 106 | thread.flags_set(threadFlag); |
altb2 | 0:d2e117716219 | 107 | } |
altb2 | 0:d2e117716219 | 108 | void ControllerLoop::start_loop(void) |
altb2 | 0:d2e117716219 | 109 | { |
altb2 | 0:d2e117716219 | 110 | thread.start(callback(this, &ControllerLoop::loop)); |
altb2 | 0:d2e117716219 | 111 | ticker.attach(callback(this, &ControllerLoop::sendSignal), Ts); |
altb2 | 0:d2e117716219 | 112 | } |
altb2 | 0:d2e117716219 | 113 | |
altb2 | 0:d2e117716219 | 114 | float ControllerLoop::pos_cntrl(float d_phi) |
altb2 | 0:d2e117716219 | 115 | { |
altb2 | 0:d2e117716219 | 116 | |
altb2 | 0:d2e117716219 | 117 | // write position controller here |
altb2 | 0:d2e117716219 | 118 | return 0.0; |
altb2 | 0:d2e117716219 | 119 | } |
altb2 | 0:d2e117716219 | 120 | |
altb2 | 0:d2e117716219 | 121 | void ControllerLoop::init_controllers(void) |
altb2 | 0:d2e117716219 | 122 | { |
altb2 | 0:d2e117716219 | 123 | // set values for your velocity and position controller here! |
altb2 | 0:d2e117716219 | 124 | |
altb2 | 0:d2e117716219 | 125 | |
altb2 | 0:d2e117716219 | 126 | } |
altb2 | 0:d2e117716219 | 127 | // find_index: move axis slowly to detect the zero-pulse |
altb2 | 0:d2e117716219 | 128 | void ControllerLoop::find_index(void) |
altb2 | 0:d2e117716219 | 129 | { |
altb2 | 0:d2e117716219 | 130 | // use a simple P-controller to get system spinning, add a constant current to overcome friction |
altb2 | 0:d2e117716219 | 131 | float Kp = 0.005; |
altb2 | 0:d2e117716219 | 132 | float i1 = 0.2f + Kp*(50.0f - data.sens_Vphi[0]); |
altb2 | 0:d2e117716219 | 133 | float i2 = 0.2f + Kp*(50.0f - data.sens_Vphi[1]) ; |
altb2 | 0:d2e117716219 | 134 | i_des1.write(i2u(i1)); |
altb2 | 0:d2e117716219 | 135 | i_des2.write(i2u(i2)); |
altb2 | 2:c4c4cc1bff45 | 136 | } |
altb2 | 2:c4c4cc1bff45 | 137 | |
altb2 | 2:c4c4cc1bff45 | 138 | void ControllerLoop::reset_pids(void) |
altb2 | 2:c4c4cc1bff45 | 139 | { |
altb2 | 2:c4c4cc1bff45 | 140 | // reset all cntrls. |
altb2 | 2:c4c4cc1bff45 | 141 | |
altb2 | 2:c4c4cc1bff45 | 142 | |
altb2 | 0:d2e117716219 | 143 | } |