Ruprecht Altenburger
/
mirror_actuator_V1
Mirror actuator for RT2 lab
ControllerLoop.cpp
- Committer:
- altb2
- Date:
- 2021-04-28
- Revision:
- 10:bfacffec199a
- Parent:
- 8:49ac75c42da0
- Child:
- 11:d43f8b421d6d
File content as of revision 10:bfacffec199a:
#include "ControllerLoop.h" using namespace std; // contructor for controller loop ControllerLoop::ControllerLoop(float Ts) : thread(osPriorityHigh,4096), dout1(PB_9) { this->Ts = Ts; diff1.reset(0.0f,0); diff2.reset(0.0f,0); is_initialized = false; ti.reset(); ti.start(); data.laser_on = false; } // decontructor for controller loop ControllerLoop::~ControllerLoop() {} // ---------------------------------------------------------------------------- // this is the main loop called every Ts with high priority void ControllerLoop::loop(void){ float w01=2*3.1415927 * 8; float xy[2]; while(1) { ThisThread::flags_wait_any(threadFlag); // THE LOOP ------------------------------------------------------------ short c1 = counter1 - index1.positionAtIndexPulse - mk.inc_offset[0]- mk.inc_additional_offset[0]; // get counts from Encoder short c2 = counter2 - index2.positionAtIndexPulse - mk.inc_offset[1]- mk.inc_additional_offset[1]; // get counts from Encoder data.sens_phi[0] = uw2pi1(2.0f*3.1415927f/4000.0f*(float)c1); data.sens_Vphi[0] = diff1(c1); // motor velocity data.sens_phi[1] = uw2pi2(2.0f*3.1415927f/4000.0f*(float)c2); data.sens_Vphi[1] = diff2(c2); // motor velocity // ------------------------------------------------------------- // at very beginning: move system slowly to find the zero pulse // set "if(0)" if you like to ommit at beginning if(!is_initialized) { find_index(); if(index1.positionAtIndexPulse != 0 && index2.positionAtIndexPulse != 0) is_initialized=true; } 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] ) ; // ------------------------ 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())); // now do trafos etc if(mk.external_control) // get desired values from external source (GUI) { if(mk.trafo_is_on) // use desired xy values from xternal source and transform // otherwise external source delivers phi1, phi2 values directly { bool dum = mk.X2P(data.cntrl_xy_des,data.cntrl_phi_des); } } else { if(mk.trafo_is_on) { data.cntrl_xy_des[0] = 30.0f*cosf(w01*glob_ti.read()); 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[1] = .250f*sinf(w01*glob_ti.read()); } } //bool dum = mk.P2X(data.cntrl_phi_des,data.cntrl_xy_des) //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; } laser_on = data.laser_on; i_enable = big_button; } } void ControllerLoop::sendSignal() { thread.flags_set(threadFlag); } void ControllerLoop::start_loop(void) { thread.start(callback(this, &ControllerLoop::loop)); ticker.attach(callback(this, &ControllerLoop::sendSignal), Ts); } float ControllerLoop::pos_cntrl(float d_phi) { return Kv*mk.mot_inc_to_rad*round(mk.mot_rad_to_inc*(d_phi)); } 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); } // 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]) ; i_des1.write(i2u(i1)); i_des2.write(i2u(i2)); }