Ruprecht Altenburger / Mbed OS simple_program

Simple program for introduction of mirror actuator.

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?

UserRevisionLine numberNew 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 }