Ruprecht Altenburger / Mbed OS mirror_actuator_V1

Mirror actuator for RT2 lab

Dependencies:   FastPWM

ControllerLoop.cpp@8:49ac75c42da0, 2021-04-27 (annotated)

Committer:
altb2
Date:
Tue Apr 27 07:50:03 2021 +0000
Revision:
8:49ac75c42da0
Parent:
7:942fd77d5e19
Child:
10:bfacffec199a
intermediate;

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 7:942fd77d5e19 6 ControllerLoop::ControllerLoop(float Ts) : thread(osPriorityNormal,4096), dout1(PB_9)
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 7:942fd77d5e19 11 Kv = 150;
altb2 6:9ebeffe446e4 12 is_initialized = false;
altb2 6:9ebeffe446e4 13 ti.reset();
altb2 6:9ebeffe446e4 14 ti.start();
altb2 7:942fd77d5e19 15 data.laser_on = false;
altb2 5:768e10f6d372 16 }
altb2 5:768e10f6d372 17
altb2 5:768e10f6d372 18
altb2 5:768e10f6d372 19 ControllerLoop::~ControllerLoop() {}
altb2 5:768e10f6d372 20
altb2 5:768e10f6d372 21
altb2 5:768e10f6d372 22 void ControllerLoop::loop(void){
altb2 6:9ebeffe446e4 23 float A=.6;
altb2 6:9ebeffe446e4 24 uint32_t k=0;
altb2 8:49ac75c42da0 25 float exc = 0.0;
altb2 8:49ac75c42da0 26 float w01=2*3.1415927 * 8;
altb2 8:49ac75c42da0 27 float w02=2*3.1415927 * 1.5;
altb2 8:49ac75c42da0 28 float xy[2];
altb2 5:768e10f6d372 29 while(1)
altb2 5:768e10f6d372 30 {
altb2 5:768e10f6d372 31 ThisThread::flags_wait_any(threadFlag);
altb2 5:768e10f6d372 32 // THE LOOP ------------------------------------------------------------
altb2 6:9ebeffe446e4 33 if(++k%500==0)
altb2 5:768e10f6d372 34 {
altb2 6:9ebeffe446e4 35 // short c1 = counter1; // get counts from Encoder
altb2 6:9ebeffe446e4 36 // short c2 = counter2; // get counts from Encoder
altb2 6:9ebeffe446e4 37
altb2 6:9ebeffe446e4 38 // 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 39 //led1=!led1;
altb2 6:9ebeffe446e4 40 }
altb2 7:942fd77d5e19 41 if(!is_initialized)
altb2 6:9ebeffe446e4 42 {
altb2 6:9ebeffe446e4 43 find_index();
altb2 6:9ebeffe446e4 44 if(index1.positionAtIndexPulse != 0 && index2.positionAtIndexPulse != 0)
altb2 6:9ebeffe446e4 45 is_initialized=true;
altb2 5:768e10f6d372 46 }
altb2 6:9ebeffe446e4 47 else
altb2 6:9ebeffe446e4 48 {
altb2 7:942fd77d5e19 49 short c1 = counter1 - index1.positionAtIndexPulse - mk.inc_offset[0]- mk.inc_additional_offset[0]; // get counts from Encoder
altb2 7:942fd77d5e19 50 short c2 = counter2 - index2.positionAtIndexPulse - mk.inc_offset[1]- mk.inc_additional_offset[1]; // get counts from Encoder
altb2 8:49ac75c42da0 51 data.sens_phi[0] = uw2pi1(2.0f*3.1415927f/4000.0f*(float)c1);
altb2 6:9ebeffe446e4 52 data.sens_Vphi[0] = diff1(c1); // motor velocity
altb2 8:49ac75c42da0 53 data.sens_phi[1] = uw2pi2(2.0f*3.1415927f/4000.0f*(float)c2);
altb2 7:942fd77d5e19 54 data.sens_Vphi[1] = diff2(c2); // motor velocity
altb2 7:942fd77d5e19 55 // ------------------------ do the control first
altb2 7:942fd77d5e19 56 float v_des1 = pos_cntrl(data.cntrl_phi_des[0]-data.sens_phi[0]);
altb2 8:49ac75c42da0 57 float v_des2 = pos_cntrl(data.cntrl_phi_des[1]-data.sens_phi[1]);//-150.0f*(1+.5f*cosf(w01*glob_ti.read()));
altb2 7:942fd77d5e19 58 data.i_des[0] = v_cntrl[0](v_des1 - data.sens_Vphi[0] ) ;
altb2 7:942fd77d5e19 59 data.i_des[1] = v_cntrl[1](v_des2 - data.sens_Vphi[1] ) ;
altb2 7:942fd77d5e19 60 // ------------------------ write outputs
altb2 7:942fd77d5e19 61 i_des1.write(i2u(data.i_des[0]));
altb2 7:942fd77d5e19 62 i_des2.write(i2u(data.i_des[1]));
altb2 8:49ac75c42da0 63 //exc =myGPA(data.i_des[1],data.sens_Vphi[1]);
altb2 8:49ac75c42da0 64 //exc = myGPA(data.i_des[1],u2i(i_act2.read()));
altb2 7:942fd77d5e19 65 // now do trafos etc
altb2 7:942fd77d5e19 66
altb2 7:942fd77d5e19 67 if(mk.external_control) // get desired values from external source (GUI)
altb2 7:942fd77d5e19 68 {
altb2 7:942fd77d5e19 69 if(mk.trafo_is_on) // use desired xy values from xternal source and transform
altb2 7:942fd77d5e19 70 // otherwise external source delivers phi1, phi2 values directly
altb2 7:942fd77d5e19 71 {
altb2 7:942fd77d5e19 72 bool dum = mk.X2P(data.cntrl_xy_des,data.cntrl_phi_des);
altb2 7:942fd77d5e19 73 }
altb2 7:942fd77d5e19 74 }
altb2 7:942fd77d5e19 75 else
altb2 7:942fd77d5e19 76 {
altb2 7:942fd77d5e19 77 if(mk.trafo_is_on)
altb2 7:942fd77d5e19 78 {
altb2 8:49ac75c42da0 79 data.cntrl_xy_des[0] = 30.0f*cosf(w01*glob_ti.read());
altb2 8:49ac75c42da0 80 data.cntrl_xy_des[1] = 30.0f*sinf(w01*glob_ti.read());
altb2 7:942fd77d5e19 81 bool dum = mk.X2P(data.cntrl_xy_des,data.cntrl_phi_des);
altb2 7:942fd77d5e19 82 }
altb2 7:942fd77d5e19 83 else
altb2 7:942fd77d5e19 84 {
altb2 8:49ac75c42da0 85 data.cntrl_phi_des[0] = .250f*cosf(w01*glob_ti.read());
altb2 8:49ac75c42da0 86 data.cntrl_phi_des[1] = .250f*sinf(w01*glob_ti.read());
altb2 7:942fd77d5e19 87 }
altb2 7:942fd77d5e19 88 }
altb2 7:942fd77d5e19 89 //bool dum = mk.P2X(data.cntrl_phi_des,data.cntrl_xy_des)
altb2 6:9ebeffe446e4 90 //current_path->get_x_v(glob_ti.read(),&phi_des,&v_des);
altb2 6:9ebeffe446e4 91
altb2 7:942fd77d5e19 92 //data.cntrl_phi_des[0] = 0.0f*A*sinf(w01*ti.read());
altb2 7:942fd77d5e19 93 //data.cntrl_phi_des[1] = 0.0f*A*sinf(w02*ti.read());
altb2 7:942fd77d5e19 94 //data.est_xy[0]=data.cntrl_phi_des[0]; // temporary
altb2 7:942fd77d5e19 95 //data.est_xy[1]=data.cntrl_phi_des[1];
altb2 6:9ebeffe446e4 96 //laser_on = 1;
altb2 6:9ebeffe446e4 97 }
altb2 8:49ac75c42da0 98 laser_on = data.laser_on;
altb2 8:49ac75c42da0 99 i_enable = big_button;
altb2 5:768e10f6d372 100 }
altb2 5:768e10f6d372 101 }
altb2 5:768e10f6d372 102
altb2 5:768e10f6d372 103 void ControllerLoop::sendSignal() {
altb2 5:768e10f6d372 104 thread.flags_set(threadFlag);
altb2 5:768e10f6d372 105 }
altb2 6:9ebeffe446e4 106 void ControllerLoop::start_loop(void)
altb2 6:9ebeffe446e4 107 {
altb2 6:9ebeffe446e4 108 thread.start(callback(this, &ControllerLoop::loop));
altb2 6:9ebeffe446e4 109 ticker.attach(callback(this, &ControllerLoop::sendSignal), Ts);
altb2 6:9ebeffe446e4 110 }
altb2 7:942fd77d5e19 111
altb2 7:942fd77d5e19 112 float ControllerLoop::pos_cntrl(float d_phi)
altb2 7:942fd77d5e19 113 {
altb2 7:942fd77d5e19 114 return Kv*mk.mot_inc_to_rad*round(mk.mot_rad_to_inc*(d_phi));
altb2 7:942fd77d5e19 115 }
altb2 7:942fd77d5e19 116
altb2 6:9ebeffe446e4 117 void ControllerLoop::init_controllers(void)
altb2 6:9ebeffe446e4 118 {
altb2 8:49ac75c42da0 119 float Kp = 0.0158;//100 * 1.7173e-06/0.03f; // XX * J/km
altb2 8:49ac75c42da0 120 float TroV = 1.0f / (2.0f * 3.1415f * 250.0f);
altb2 8:49ac75c42da0 121 float Tn = .008f;
altb2 6:9ebeffe446e4 122 v_cntrl[0].setCoefficients(Kp,Kp/Tn,0.0f,1.0,TroV,Ts,-.8,.8);
altb2 6:9ebeffe446e4 123 v_cntrl[1].setCoefficients(Kp,Kp/Tn,0.0f,1.0,TroV,Ts,-.8,.8);
altb2 6:9ebeffe446e4 124 }
altb2 6:9ebeffe446e4 125 void ControllerLoop::find_index(void)
altb2 6:9ebeffe446e4 126 {
altb2 6:9ebeffe446e4 127 float Kp = 0.005;
altb2 6:9ebeffe446e4 128 short counts1 = counter1; // get counts from Encoder
altb2 6:9ebeffe446e4 129 float vel1 = diff1(counts1); // motor velocity
altb2 6:9ebeffe446e4 130 short counts2 = counter2; // get counts from Encoder
altb2 6:9ebeffe446e4 131 float vel2 = diff2(counts2); // motor velocity
altb2 6:9ebeffe446e4 132 float i1 = Kp*(25.0f - vel1 );
altb2 6:9ebeffe446e4 133 float i2 = Kp*(25.0f - vel2 ) ;
altb2 6:9ebeffe446e4 134 //float dum = (float)(++u_test%16)/16.0f;
altb2 6:9ebeffe446e4 135 //i_des1.write(i2u(.25f*sin(2*3.14f*1.0f)));
altb2 7:942fd77d5e19 136 i_des1.write(i2u(i1));
altb2 6:9ebeffe446e4 137 i_des2.write(i2u(i2));
altb2 6:9ebeffe446e4 138 }