Mirror actuator for RT2 lab
Dependencies: Library_Cntrl Library_Misc
Diff: ControllerLoop.cpp
- Revision:
- 7:942fd77d5e19
- Parent:
- 6:9ebeffe446e4
--- a/ControllerLoop.cpp Thu Apr 01 13:38:54 2021 +0000 +++ b/ControllerLoop.cpp Thu Apr 15 05:36:55 2021 +0000 @@ -3,14 +3,16 @@ -ControllerLoop::ControllerLoop(float Ts) : thread(osPriorityNormal,4096) +ControllerLoop::ControllerLoop(float Ts) : thread(osPriorityNormal,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; } @@ -20,7 +22,6 @@ void ControllerLoop::loop(void){ float A=.6; uint32_t k=0; - float Kv = 150; while(1) { ThisThread::flags_wait_any(threadFlag); @@ -33,7 +34,7 @@ // 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; } - if(0)//!is_initialized) + if(!is_initialized) { find_index(); if(index1.positionAtIndexPulse != 0 && index2.positionAtIndexPulse != 0) @@ -41,31 +42,56 @@ } else { - short c1 = counter1 - index1.positionAtIndexPulse - mc.inc_offset[0]- mc.inc_additional_offset[0]; // get counts from Encoder - short c2 = counter2 - index2.positionAtIndexPulse - mc.inc_offset[1]- mc.inc_additional_offset[1]; // get counts from Encoder + 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*3.1415927/4000.0*(float)c1); data.sens_Vphi[0] = diff1(c1); // motor velocity data.sens_phi[1] = uw2pi2(2*3.1415927/4000.0*(float)c2); - data.sens_Vphi[1] = diff2(c2); // motor velocity - float w01=2*3.1415927 * 3; + 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]); + 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])); + // now do trafos etc + + float w01=2*3.1415927 * .5; float w02=2*3.1415927 * 1.5; - + float xy[2]; + 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] = 50.0f*cosf(w01*glob_ti.read()); + data.cntrl_xy_des[1] = 50.0f*sinf(w01*glob_ti.read()); + bool dum = mk.X2P(data.cntrl_xy_des,data.cntrl_phi_des); + } + else + { + data.cntrl_phi_des[0] = .50f*cosf(w01*glob_ti.read()); + data.cntrl_phi_des[1] = .50f*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]; + //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; - - float v_des1 = Kv*(data.cntrl_phi_des[0]-data.sens_phi[0]); - float v_des2 = Kv*(data.cntrl_phi_des[1]-data.sens_phi[1]); - 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] ) ; - //float dum = (float)(++u_test%16)/16.0f; - //i_des1.write(i2u(.25f*sin(2*3.14f*1.0f))); - //i_des1.write(i2u(data.i_des[0])); - i_des2.write(i2u(data.i_des[1])); + laser_on = data.laser_on; } } } @@ -78,6 +104,12 @@ 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 = 2000 * 4.89e-7/0.094f; // XX * J/km @@ -97,6 +129,6 @@ 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))); - //i_des1.write(i2u(i1)); + i_des1.write(i2u(i1)); i_des2.write(i2u(i2)); } \ No newline at end of file