Mirror actuator for RT2 lab
Dependencies: Library_Cntrl Library_Misc
Diff: ControllerLoop.cpp
- Revision:
- 5:768e10f6d372
- Child:
- 6:9ebeffe446e4
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ControllerLoop.cpp Thu Feb 25 20:28:45 2021 +0000 @@ -0,0 +1,52 @@ +#include "ControllerLoop.h" +using namespace std; + + + +ControllerLoop::ControllerLoop(float Ts) : thread(osPriorityNormal,4096) +{ + thread.start(callback(this, &ControllerLoop::loop)); + ticker.attach(callback(this, &ControllerLoop::sendSignal), Ts); + diff1.reset(0.0f,0); + diff2.reset(0.0f,0); + } + + +ControllerLoop::~ControllerLoop() {} + + +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; + while(1) + { + ThisThread::flags_wait_any(threadFlag); + // THE LOOP ------------------------------------------------------------ + if(++k%64==0) + { + printf("%f %f\r\n",phi_des,v_des); + } + 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)); + } +} + +void ControllerLoop::sendSignal() { + thread.flags_set(threadFlag); +}