Ruprecht Altenburger / Mbed OS mirror_actuator_stud

template for students for mirror actuator

Dependencies:   FastPWM

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?

UserRevisionLine numberNew 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 }