Mirror actuator for RT2 lab
Dependencies: Library_Cntrl Library_Misc
ControllerLoop.cpp
- Committer:
- altb2
- Date:
- 2021-02-25
- Revision:
- 5:768e10f6d372
- Child:
- 6:9ebeffe446e4
File content as of revision 5:768e10f6d372:
#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); }