Ruprecht Altenburger / Mbed OS simple_program

Simple program for introduction of mirror actuator.

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?

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