Mirror actuator for RT2 lab
Dependencies: Library_Cntrl Library_Misc
ControllerLoop.cpp@5:768e10f6d372, 2021-02-25 (annotated)
- Committer:
- altb2
- Date:
- Thu Feb 25 20:28:45 2021 +0000
- Revision:
- 5:768e10f6d372
- Child:
- 6:9ebeffe446e4
first commit of Mirror actuator
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 | 5:768e10f6d372 | 8 | thread.start(callback(this, &ControllerLoop::loop)); |
altb2 | 5:768e10f6d372 | 9 | ticker.attach(callback(this, &ControllerLoop::sendSignal), Ts); |
altb2 | 5:768e10f6d372 | 10 | diff1.reset(0.0f,0); |
altb2 | 5:768e10f6d372 | 11 | diff2.reset(0.0f,0); |
altb2 | 5:768e10f6d372 | 12 | } |
altb2 | 5:768e10f6d372 | 13 | |
altb2 | 5:768e10f6d372 | 14 | |
altb2 | 5:768e10f6d372 | 15 | ControllerLoop::~ControllerLoop() {} |
altb2 | 5:768e10f6d372 | 16 | |
altb2 | 5:768e10f6d372 | 17 | |
altb2 | 5:768e10f6d372 | 18 | void ControllerLoop::loop(void){ |
altb2 | 5:768e10f6d372 | 19 | float Kp = 1500 * 4.89e-7/0.094; // XX * J/km |
altb2 | 5:768e10f6d372 | 20 | float Tn = .0035; |
altb2 | 5:768e10f6d372 | 21 | float Kv=50; |
altb2 | 5:768e10f6d372 | 22 | float phi_des = 0; |
altb2 | 5:768e10f6d372 | 23 | float v_des = 0; |
altb2 | 5:768e10f6d372 | 24 | PID_Cntrl v_cntrl(Kp,Tn,0,1.0,Ts,-.95,.95); |
altb2 | 5:768e10f6d372 | 25 | Unwrapper_2pi uw2pi; |
altb2 | 5:768e10f6d372 | 26 | uint8_t k = 0; |
altb2 | 5:768e10f6d372 | 27 | while(1) |
altb2 | 5:768e10f6d372 | 28 | { |
altb2 | 5:768e10f6d372 | 29 | ThisThread::flags_wait_any(threadFlag); |
altb2 | 5:768e10f6d372 | 30 | // THE LOOP ------------------------------------------------------------ |
altb2 | 5:768e10f6d372 | 31 | if(++k%64==0) |
altb2 | 5:768e10f6d372 | 32 | { |
altb2 | 5:768e10f6d372 | 33 | printf("%f %f\r\n",phi_des,v_des); |
altb2 | 5:768e10f6d372 | 34 | } |
altb2 | 5:768e10f6d372 | 35 | short c1 = counter1; // get counts from Encoder |
altb2 | 5:768e10f6d372 | 36 | short c2 = counter2; // get counts from Encoder |
altb2 | 5:768e10f6d372 | 37 | float phi1 = uw2pi(2*3.1415927/4000.0*(float)c1); |
altb2 | 5:768e10f6d372 | 38 | float vel1 = diff1(c1); // motor velocity |
altb2 | 5:768e10f6d372 | 39 | float phi2 = uw2pi(2*3.1415927/4000.0*(float)c2); |
altb2 | 5:768e10f6d372 | 40 | float vel2 = diff2(c2); // motor velocity |
altb2 | 5:768e10f6d372 | 41 | float w0=2*3.1415927 * 3; |
altb2 | 5:768e10f6d372 | 42 | //phi_des = 1.0*sinf(w0*ti.read()); |
altb2 | 5:768e10f6d372 | 43 | current_path->get_x_v(glob_ti.read(),&phi_des,&v_des); |
altb2 | 5:768e10f6d372 | 44 | float i1 = v_cntrl(Kv * (phi_des-phi2)+v_des-vel2 ); |
altb2 | 5:768e10f6d372 | 45 | //float i1 = v_cntrl(v_des-vel ) + exc; |
altb2 | 5:768e10f6d372 | 46 | i_des2.write(i2pwm(-i1)); |
altb2 | 5:768e10f6d372 | 47 | } |
altb2 | 5:768e10f6d372 | 48 | } |
altb2 | 5:768e10f6d372 | 49 | |
altb2 | 5:768e10f6d372 | 50 | void ControllerLoop::sendSignal() { |
altb2 | 5:768e10f6d372 | 51 | thread.flags_set(threadFlag); |
altb2 | 5:768e10f6d372 | 52 | } |