Ruprecht Altenburger
/
mirror_actuator_V1
Mirror actuator for RT2 lab
Diff: ControllerLoop.cpp
- Revision:
- 10:bfacffec199a
- Parent:
- 8:49ac75c42da0
- Child:
- 11:d43f8b421d6d
--- a/ControllerLoop.cpp Tue Apr 27 07:50:50 2021 +0000 +++ b/ControllerLoop.cpp Wed Apr 28 08:26:37 2021 +0000 @@ -1,43 +1,39 @@ #include "ControllerLoop.h" using namespace std; - - -ControllerLoop::ControllerLoop(float Ts) : thread(osPriorityNormal,4096), dout1(PB_9) +// 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); - Kv = 150; 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 A=.6; - uint32_t k=0; - float exc = 0.0; float w01=2*3.1415927 * 8; - float w02=2*3.1415927 * 1.5; float xy[2]; while(1) { ThisThread::flags_wait_any(threadFlag); // THE LOOP ------------------------------------------------------------ - if(++k%500==0) - { -// short c1 = counter1; // get counts from Encoder -// short c2 = counter2; // get counts from Encoder - - // printf("1: %d %d, 2: %d %d\r\n",index1.positionAtIndexPulse,c1-index1.positionAtIndexPulse-mc.inc_offset[0],index2.positionAtIndexPulse,c2-index2.positionAtIndexPulse-mc.inc_offset[1]); - //led1=!led1; - } + 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(); @@ -46,12 +42,6 @@ } else { - 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 // ------------------------ 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())); @@ -122,17 +112,13 @@ 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; - short counts1 = counter1; // get counts from Encoder - float vel1 = diff1(counts1); // motor velocity - short counts2 = counter2; // get counts from Encoder - float vel2 = diff2(counts2); // motor velocity - float i1 = Kp*(25.0f - vel1 ); - float i2 = Kp*(25.0f - vel2 ) ; - //float dum = (float)(++u_test%16)/16.0f; - //i_des1.write(i2u(.25f*sin(2*3.14f*1.0f))); + 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)); } \ No newline at end of file