Ruprecht Altenburger
/
mirror_actuator_V1
Mirror actuator for RT2 lab
Diff: ControllerLoop.cpp
- Revision:
- 6:9ebeffe446e4
- Parent:
- 5:768e10f6d372
- Child:
- 7:942fd77d5e19
diff -r 768e10f6d372 -r 9ebeffe446e4 ControllerLoop.cpp --- a/ControllerLoop.cpp Thu Feb 25 20:28:45 2021 +0000 +++ b/ControllerLoop.cpp Thu Apr 01 13:38:54 2021 +0000 @@ -5,10 +5,12 @@ ControllerLoop::ControllerLoop(float Ts) : thread(osPriorityNormal,4096) { - thread.start(callback(this, &ControllerLoop::loop)); - ticker.attach(callback(this, &ControllerLoop::sendSignal), Ts); + this->Ts = Ts; diff1.reset(0.0f,0); diff2.reset(0.0f,0); + is_initialized = false; + ti.reset(); + ti.start(); } @@ -16,37 +18,85 @@ void ControllerLoop::loop(void){ - float Kp = 1500 * 4.89e-7/0.094; // XX * J/km - float Tn = .0035; - float Kv=50; - float phi_des = 0; - float v_des = 0; - PID_Cntrl v_cntrl(Kp,Tn,0,1.0,Ts,-.95,.95); - Unwrapper_2pi uw2pi; - uint8_t k = 0; + float A=.6; + uint32_t k=0; + float Kv = 150; while(1) { ThisThread::flags_wait_any(threadFlag); // THE LOOP ------------------------------------------------------------ - if(++k%64==0) + if(++k%500==0) { - printf("%f %f\r\n",phi_des,v_des); +// 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; + } + if(0)//!is_initialized) + { + find_index(); + if(index1.positionAtIndexPulse != 0 && index2.positionAtIndexPulse != 0) + is_initialized=true; } - short c1 = counter1; // get counts from Encoder - short c2 = counter2; // get counts from Encoder - float phi1 = uw2pi(2*3.1415927/4000.0*(float)c1); - float vel1 = diff1(c1); // motor velocity - float phi2 = uw2pi(2*3.1415927/4000.0*(float)c2); - float vel2 = diff2(c2); // motor velocity - float w0=2*3.1415927 * 3; - //phi_des = 1.0*sinf(w0*ti.read()); - current_path->get_x_v(glob_ti.read(),&phi_des,&v_des); - float i1 = v_cntrl(Kv * (phi_des-phi2)+v_des-vel2 ); - //float i1 = v_cntrl(v_des-vel ) + exc; - i_des2.write(i2pwm(-i1)); + 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 + 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; + float w02=2*3.1415927 * 1.5; + + //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; + + 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])); + } } } 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); +} +void ControllerLoop::init_controllers(void) +{ + float Kp = 2000 * 4.89e-7/0.094f; // XX * J/km + float TroV = 1.0f / (2.0f * 3.1415f * 330.0f); + float Tn = .005f; + 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); +} +void ControllerLoop::find_index(void) +{ + 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))); + //i_des1.write(i2u(i1)); + i_des2.write(i2u(i2)); + } \ No newline at end of file