Mirror actuator for RT2 lab
Dependencies: Library_Cntrl Library_Misc
ControllerLoop.cpp@6:9ebeffe446e4, 2021-04-01 (annotated)
- Committer:
- altb2
- Date:
- Thu Apr 01 13:38:54 2021 +0000
- Revision:
- 6:9ebeffe446e4
- Parent:
- 5:768e10f6d372
- Child:
- 7:942fd77d5e19
first commit
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
altb2 | 5:768e10f6d372 | 1 | #include "ControllerLoop.h" |
altb2 | 5:768e10f6d372 | 2 | using namespace std; |
altb2 | 5:768e10f6d372 | 3 | |
altb2 | 5:768e10f6d372 | 4 | |
altb2 | 5:768e10f6d372 | 5 | |
altb2 | 5:768e10f6d372 | 6 | ControllerLoop::ControllerLoop(float Ts) : thread(osPriorityNormal,4096) |
altb2 | 5:768e10f6d372 | 7 | { |
altb2 | 6:9ebeffe446e4 | 8 | this->Ts = Ts; |
altb2 | 5:768e10f6d372 | 9 | diff1.reset(0.0f,0); |
altb2 | 5:768e10f6d372 | 10 | diff2.reset(0.0f,0); |
altb2 | 6:9ebeffe446e4 | 11 | is_initialized = false; |
altb2 | 6:9ebeffe446e4 | 12 | ti.reset(); |
altb2 | 6:9ebeffe446e4 | 13 | ti.start(); |
altb2 | 5:768e10f6d372 | 14 | } |
altb2 | 5:768e10f6d372 | 15 | |
altb2 | 5:768e10f6d372 | 16 | |
altb2 | 5:768e10f6d372 | 17 | ControllerLoop::~ControllerLoop() {} |
altb2 | 5:768e10f6d372 | 18 | |
altb2 | 5:768e10f6d372 | 19 | |
altb2 | 5:768e10f6d372 | 20 | void ControllerLoop::loop(void){ |
altb2 | 6:9ebeffe446e4 | 21 | float A=.6; |
altb2 | 6:9ebeffe446e4 | 22 | uint32_t k=0; |
altb2 | 6:9ebeffe446e4 | 23 | float Kv = 150; |
altb2 | 5:768e10f6d372 | 24 | while(1) |
altb2 | 5:768e10f6d372 | 25 | { |
altb2 | 5:768e10f6d372 | 26 | ThisThread::flags_wait_any(threadFlag); |
altb2 | 5:768e10f6d372 | 27 | // THE LOOP ------------------------------------------------------------ |
altb2 | 6:9ebeffe446e4 | 28 | if(++k%500==0) |
altb2 | 5:768e10f6d372 | 29 | { |
altb2 | 6:9ebeffe446e4 | 30 | // short c1 = counter1; // get counts from Encoder |
altb2 | 6:9ebeffe446e4 | 31 | // short c2 = counter2; // get counts from Encoder |
altb2 | 6:9ebeffe446e4 | 32 | |
altb2 | 6:9ebeffe446e4 | 33 | // 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]); |
altb2 | 6:9ebeffe446e4 | 34 | //led1=!led1; |
altb2 | 6:9ebeffe446e4 | 35 | } |
altb2 | 6:9ebeffe446e4 | 36 | if(0)//!is_initialized) |
altb2 | 6:9ebeffe446e4 | 37 | { |
altb2 | 6:9ebeffe446e4 | 38 | find_index(); |
altb2 | 6:9ebeffe446e4 | 39 | if(index1.positionAtIndexPulse != 0 && index2.positionAtIndexPulse != 0) |
altb2 | 6:9ebeffe446e4 | 40 | is_initialized=true; |
altb2 | 5:768e10f6d372 | 41 | } |
altb2 | 6:9ebeffe446e4 | 42 | else |
altb2 | 6:9ebeffe446e4 | 43 | { |
altb2 | 6:9ebeffe446e4 | 44 | short c1 = counter1 - index1.positionAtIndexPulse - mc.inc_offset[0]- mc.inc_additional_offset[0]; // get counts from Encoder |
altb2 | 6:9ebeffe446e4 | 45 | short c2 = counter2 - index2.positionAtIndexPulse - mc.inc_offset[1]- mc.inc_additional_offset[1]; // get counts from Encoder |
altb2 | 6:9ebeffe446e4 | 46 | data.sens_phi[0] = uw2pi1(2*3.1415927/4000.0*(float)c1); |
altb2 | 6:9ebeffe446e4 | 47 | data.sens_Vphi[0] = diff1(c1); // motor velocity |
altb2 | 6:9ebeffe446e4 | 48 | data.sens_phi[1] = uw2pi2(2*3.1415927/4000.0*(float)c2); |
altb2 | 6:9ebeffe446e4 | 49 | data.sens_Vphi[1] = diff2(c2); // motor velocity |
altb2 | 6:9ebeffe446e4 | 50 | float w01=2*3.1415927 * 3; |
altb2 | 6:9ebeffe446e4 | 51 | float w02=2*3.1415927 * 1.5; |
altb2 | 6:9ebeffe446e4 | 52 | |
altb2 | 6:9ebeffe446e4 | 53 | //current_path->get_x_v(glob_ti.read(),&phi_des,&v_des); |
altb2 | 6:9ebeffe446e4 | 54 | |
altb2 | 6:9ebeffe446e4 | 55 | data.cntrl_phi_des[0] = 0.0f*A*sinf(w01*ti.read()); |
altb2 | 6:9ebeffe446e4 | 56 | data.cntrl_phi_des[1] = 0.0f*A*sinf(w02*ti.read()); |
altb2 | 6:9ebeffe446e4 | 57 | data.est_xy[0]=data.cntrl_phi_des[0]; // temporary |
altb2 | 6:9ebeffe446e4 | 58 | data.est_xy[1]=data.cntrl_phi_des[1]; |
altb2 | 6:9ebeffe446e4 | 59 | //laser_on = 1; |
altb2 | 6:9ebeffe446e4 | 60 | |
altb2 | 6:9ebeffe446e4 | 61 | float v_des1 = Kv*(data.cntrl_phi_des[0]-data.sens_phi[0]); |
altb2 | 6:9ebeffe446e4 | 62 | float v_des2 = Kv*(data.cntrl_phi_des[1]-data.sens_phi[1]); |
altb2 | 6:9ebeffe446e4 | 63 | data.i_des[0] = v_cntrl[0](v_des1 - data.sens_Vphi[0] ) ; |
altb2 | 6:9ebeffe446e4 | 64 | data.i_des[1] = v_cntrl[1](v_des2 - data.sens_Vphi[1] ) ; |
altb2 | 6:9ebeffe446e4 | 65 | //float dum = (float)(++u_test%16)/16.0f; |
altb2 | 6:9ebeffe446e4 | 66 | //i_des1.write(i2u(.25f*sin(2*3.14f*1.0f))); |
altb2 | 6:9ebeffe446e4 | 67 | //i_des1.write(i2u(data.i_des[0])); |
altb2 | 6:9ebeffe446e4 | 68 | i_des2.write(i2u(data.i_des[1])); |
altb2 | 6:9ebeffe446e4 | 69 | } |
altb2 | 5:768e10f6d372 | 70 | } |
altb2 | 5:768e10f6d372 | 71 | } |
altb2 | 5:768e10f6d372 | 72 | |
altb2 | 5:768e10f6d372 | 73 | void ControllerLoop::sendSignal() { |
altb2 | 5:768e10f6d372 | 74 | thread.flags_set(threadFlag); |
altb2 | 5:768e10f6d372 | 75 | } |
altb2 | 6:9ebeffe446e4 | 76 | void ControllerLoop::start_loop(void) |
altb2 | 6:9ebeffe446e4 | 77 | { |
altb2 | 6:9ebeffe446e4 | 78 | thread.start(callback(this, &ControllerLoop::loop)); |
altb2 | 6:9ebeffe446e4 | 79 | ticker.attach(callback(this, &ControllerLoop::sendSignal), Ts); |
altb2 | 6:9ebeffe446e4 | 80 | } |
altb2 | 6:9ebeffe446e4 | 81 | void ControllerLoop::init_controllers(void) |
altb2 | 6:9ebeffe446e4 | 82 | { |
altb2 | 6:9ebeffe446e4 | 83 | float Kp = 2000 * 4.89e-7/0.094f; // XX * J/km |
altb2 | 6:9ebeffe446e4 | 84 | float TroV = 1.0f / (2.0f * 3.1415f * 330.0f); |
altb2 | 6:9ebeffe446e4 | 85 | float Tn = .005f; |
altb2 | 6:9ebeffe446e4 | 86 | v_cntrl[0].setCoefficients(Kp,Kp/Tn,0.0f,1.0,TroV,Ts,-.8,.8); |
altb2 | 6:9ebeffe446e4 | 87 | v_cntrl[1].setCoefficients(Kp,Kp/Tn,0.0f,1.0,TroV,Ts,-.8,.8); |
altb2 | 6:9ebeffe446e4 | 88 | } |
altb2 | 6:9ebeffe446e4 | 89 | void ControllerLoop::find_index(void) |
altb2 | 6:9ebeffe446e4 | 90 | { |
altb2 | 6:9ebeffe446e4 | 91 | float Kp = 0.005; |
altb2 | 6:9ebeffe446e4 | 92 | short counts1 = counter1; // get counts from Encoder |
altb2 | 6:9ebeffe446e4 | 93 | float vel1 = diff1(counts1); // motor velocity |
altb2 | 6:9ebeffe446e4 | 94 | short counts2 = counter2; // get counts from Encoder |
altb2 | 6:9ebeffe446e4 | 95 | float vel2 = diff2(counts2); // motor velocity |
altb2 | 6:9ebeffe446e4 | 96 | float i1 = Kp*(25.0f - vel1 ); |
altb2 | 6:9ebeffe446e4 | 97 | float i2 = Kp*(25.0f - vel2 ) ; |
altb2 | 6:9ebeffe446e4 | 98 | //float dum = (float)(++u_test%16)/16.0f; |
altb2 | 6:9ebeffe446e4 | 99 | //i_des1.write(i2u(.25f*sin(2*3.14f*1.0f))); |
altb2 | 6:9ebeffe446e4 | 100 | //i_des1.write(i2u(i1)); |
altb2 | 6:9ebeffe446e4 | 101 | i_des2.write(i2u(i2)); |
altb2 | 6:9ebeffe446e4 | 102 | } |