Preston Ernst / Mbed OS mirror_actuator_VT

-data logging revision

Dependencies:   FastPWM

ControllerLoop.cpp@2:92c25cb669f4, 2021-08-24 (annotated)

Committer:
ernstpre
Date:
Tue Aug 24 08:51:13 2021 +0000
Revision:
2:92c25cb669f4
Parent:
1:25a2b47ca291 
Publish Commit 24/8/21

Who changed what in which revision?

UserRevisionLine numberNew contents of line
altb2 0:d2e117716219 1 #include "ControllerLoop.h"
altb2 0:d2e117716219 2 using namespace std;
altb2 0:d2e117716219 3
altb2 0:d2e117716219 4 // contructor for controller loop
altb2 0:d2e117716219 5 ControllerLoop::ControllerLoop(float Ts) : thread(osPriorityHigh,4096), dout1(PB_9)
altb2 0:d2e117716219 6 {
altb2 0:d2e117716219 7 this->Ts = Ts;
altb2 0:d2e117716219 8 diff1.reset(0.0f,0);
altb2 0:d2e117716219 9 diff2.reset(0.0f,0);
altb2 0:d2e117716219 10 is_initialized = false;
altb2 0:d2e117716219 11 ti.reset();
altb2 0:d2e117716219 12 ti.start();
altb2 0:d2e117716219 13 data.laser_on = false;
ernstpre 2:92c25cb669f4 14 }
altb2 0:d2e117716219 15
altb2 0:d2e117716219 16 // decontructor for controller loop
altb2 0:d2e117716219 17 ControllerLoop::~ControllerLoop() {}
altb2 0:d2e117716219 18
altb2 0:d2e117716219 19 // ----------------------------------------------------------------------------
altb2 0:d2e117716219 20 // this is the main loop called every Ts with high priority
ernstpre 2:92c25cb669f4 21 void ControllerLoop::loop(void)
ernstpre 2:92c25cb669f4 22 {
ernstpre 2:92c25cb669f4 23 float w01=2*3.1415927 * 2;
altb2 0:d2e117716219 24 float xy[2];
ernstpre 1:25a2b47ca291 25 float exc = 0;
ernstpre 1:25a2b47ca291 26 PID_Cntrl v_cntrl_1(0.0153f, 3.06,0,0,Ts,-0.8,0.8);
ernstpre 1:25a2b47ca291 27 PID_Cntrl v_cntrl_2(0.0153f, 3.06,0,0,Ts,-0.8,0.8);
ernstpre 2:92c25cb669f4 28
ernstpre 2:92c25cb669f4 29 bool stop_rec = false;
ernstpre 2:92c25cb669f4 30 int k=0;
ernstpre 2:92c25cb669f4 31 float Logg[2000][4]; //float datal[2000][6];
ernstpre 2:92c25cb669f4 32 //int vel1 = 5;
ernstpre 2:92c25cb669f4 33 //int vel2 =10;
ernstpre 2:92c25cb669f4 34 printf("Starting Controller \r\n");
ernstpre 2:92c25cb669f4 35 while(1) {
altb2 0:d2e117716219 36 ThisThread::flags_wait_any(threadFlag);
altb2 0:d2e117716219 37 // THE LOOP ------------------------------------------------------------
altb2 0:d2e117716219 38 short c1 = counter1 - index1.positionAtIndexPulse - mk.inc_offset[0]- mk.inc_additional_offset[0]; // get counts from Encoder
altb2 0:d2e117716219 39 short c2 = counter2 - index2.positionAtIndexPulse - mk.inc_offset[1]- mk.inc_additional_offset[1]; // get counts from Encoder
altb2 0:d2e117716219 40 data.sens_phi[0] = uw2pi1(2.0f*3.1415927f/4000.0f*(float)c1);
ernstpre 2:92c25cb669f4 41 data.sens_Vphi[0] = diff1(c1); // motor velocity
altb2 0:d2e117716219 42 data.sens_phi[1] = uw2pi2(2.0f*3.1415927f/4000.0f*(float)c2);
altb2 0:d2e117716219 43 data.sens_Vphi[1] = diff2(c2); // motor velocity
altb2 0:d2e117716219 44 // -------------------------------------------------------------
altb2 0:d2e117716219 45 // at very beginning: move system slowly to find the zero pulse
altb2 0:d2e117716219 46 // set "if(0)" if you like to ommit at beginning
ernstpre 2:92c25cb669f4 47 if(!is_initialized) {
altb2 0:d2e117716219 48 find_index();
ernstpre 2:92c25cb669f4 49 if(index1.positionAtIndexPulse != 0 && index2.positionAtIndexPulse != 0)
altb2 0:d2e117716219 50 is_initialized=true;
ernstpre 2:92c25cb669f4 51 } else {
ernstpre 2:92c25cb669f4 52 if(k==0)
ernstpre 2:92c25cb669f4 53 {
ernstpre 2:92c25cb669f4 54 printf("Starting else loop \r\n");
altb2 0:d2e117716219 55 }
ernstpre 1:25a2b47ca291 56 // float Kp = 0.005;
ernstpre 1:25a2b47ca291 57 // data.i_des[0] = 0.1f + Kp*(exc+50.0f - data.sens_Vphi[0]);
altb2 0:d2e117716219 58 // ------------------------ do the control first
altb2 0:d2e117716219 59
ernstpre 2:92c25cb669f4 60 // calculate desired currents here, you can do "anything" here,
altb2 0:d2e117716219 61 // if you like to refer to values e.g. from the gui or from the trafo,
altb2 0:d2e117716219 62 // please use data.xxx values, they are calculated 30 lines below
ernstpre 2:92c25cb669f4 63 //float e1 = 50 - data.sens_Vphi[0];
ernstpre 2:92c25cb669f4 64 //float e2 = 50 - data.sens_Vphi[1];
ernstpre 2:92c25cb669f4 65 //float v_des1 = exc;
ernstpre 2:92c25cb669f4 66 //float v_des2 = 0;
ernstpre 2:92c25cb669f4 67 float phi1_des = 0.3f*sinf(2.0f* 3.14159f*2.0f*ti.read());
ernstpre 2:92c25cb669f4 68 float phi2_des = 0.3f*cosf(2.0f* 3.14159f*2.0f*ti.read());
ernstpre 2:92c25cb669f4 69 float Kv = 123;
ernstpre 2:92c25cb669f4 70 float v_des1 = Kv*(phi1_des - data.sens_phi[0]);
ernstpre 2:92c25cb669f4 71 float v_des2 = Kv*(phi2_des - data.sens_phi[1]);
ernstpre 2:92c25cb669f4 72 data.i_des[0] = v_cntrl_1(v_des1 - data.sens_Vphi[0]);
ernstpre 2:92c25cb669f4 73 data.i_des[1] = v_cntrl_2(v_des2 - data.sens_Vphi[1]);
ernstpre 2:92c25cb669f4 74
ernstpre 2:92c25cb669f4 75 //data.i_des[1] =0.0;
ernstpre 2:92c25cb669f4 76
altb2 0:d2e117716219 77 // ------------------------ write outputs
altb2 0:d2e117716219 78 i_des1.write(i2u(data.i_des[0]));
altb2 0:d2e117716219 79 i_des2.write(i2u(data.i_des[1]));
altb2 0:d2e117716219 80 // GPA: if you want to use the GPA, uncomment and improve following line:
ernstpre 1:25a2b47ca291 81 //exc = myGPA(data.i_des[0],data.sens_Vphi[0]);
ernstpre 1:25a2b47ca291 82 exc = myGPA(v_des1, data.sens_phi[0]);
ernstpre 2:92c25cb669f4 83 //
ernstpre 2:92c25cb669f4 84
ernstpre 2:92c25cb669f4 85 /*if(k%10000==0) {
ernstpre 2:92c25cb669f4 86 printf("yes \n");
ernstpre 2:92c25cb669f4 87 //printf("c1: %d c2: %d i2: %f\r\n",counts1,counts2,i2);
ernstpre 2:92c25cb669f4 88 //printf("p1: %f p2: %f pd1: %f pd2: %f id1: %f id2: %f\r\n",data.sens_phi[0],data.sens_phi[1],phi1_des,phi2_des,data.i_des[0],data.i_des[1]);
ernstpre 2:92c25cb669f4 89 }*/
ernstpre 2:92c25cb669f4 90
ernstpre 2:92c25cb669f4 91 if(k==200 && !stop_rec)
ernstpre 2:92c25cb669f4 92 {
ernstpre 2:92c25cb669f4 93 stop_rec = true;
ernstpre 2:92c25cb669f4 94 k=0;
ernstpre 2:92c25cb669f4 95
ernstpre 2:92c25cb669f4 96 for(int k1=0; k1<2000; k1++)
ernstpre 2:92c25cb669f4 97 {
ernstpre 2:92c25cb669f4 98 for(int k2=0; k2<4; k2++)
ernstpre 2:92c25cb669f4 99 {
ernstpre 2:92c25cb669f4 100 //printf("k1 = %d k2 = %d \r\n", k1, k2);
ernstpre 2:92c25cb669f4 101 printf("%3.4f ",Logg[k1][k2]);
ernstpre 2:92c25cb669f4 102 }
ernstpre 2:92c25cb669f4 103 printf("\r\n");
ernstpre 2:92c25cb669f4 104 }
ernstpre 2:92c25cb669f4 105 }
ernstpre 2:92c25cb669f4 106
ernstpre 2:92c25cb669f4 107 if(k<2000 && !stop_rec)
ernstpre 2:92c25cb669f4 108 {
ernstpre 2:92c25cb669f4 109 Logg[k][0]=data.sens_phi[0];
ernstpre 2:92c25cb669f4 110 Logg[k][1]=data.sens_phi[1];
ernstpre 2:92c25cb669f4 111 Logg[k][2]=data.i_des[0];
ernstpre 2:92c25cb669f4 112 Logg[k][3]=data.i_des[1];
ernstpre 2:92c25cb669f4 113 }
ernstpre 2:92c25cb669f4 114 k++;
ernstpre 2:92c25cb669f4 115 //
ernstpre 2:92c25cb669f4 116
altb2 0:d2e117716219 117 // now do trafos etc
altb2 0:d2e117716219 118
ernstpre 2:92c25cb669f4 119 if(mk.external_control) { // get desired values from external source (GUI)
altb2 0:d2e117716219 120 if(mk.trafo_is_on) // use desired xy values from xternal source and transform
ernstpre 2:92c25cb669f4 121 // otherwise external source delivers phi1, phi2 values directly
ernstpre 2:92c25cb669f4 122 {
altb2 0:d2e117716219 123 bool dum = mk.X2P(data.cntrl_xy_des,data.cntrl_phi_des);
altb2 0:d2e117716219 124 }
ernstpre 2:92c25cb669f4 125 } else { // this is called, when desired values are calculated here internally (e.g. pathplanner)
ernstpre 2:92c25cb669f4 126 if(mk.trafo_is_on) {
altb2 0:d2e117716219 127 data.cntrl_xy_des[0] = 30.0f*cosf(w01*glob_ti.read()); // make a circle in xy-co-ordinates
altb2 0:d2e117716219 128 data.cntrl_xy_des[1] = 30.0f*sinf(w01*glob_ti.read());
altb2 0:d2e117716219 129 bool dum = mk.X2P(data.cntrl_xy_des,data.cntrl_phi_des);
ernstpre 2:92c25cb669f4 130 } else {
altb2 0:d2e117716219 131 data.cntrl_phi_des[0] = .250f*cosf(w01*glob_ti.read()); // make some harmonic movements directly on phi1/phi2
altb2 0:d2e117716219 132 data.cntrl_phi_des[1] = .250f*sinf(w01*glob_ti.read());
altb2 0:d2e117716219 133 }
ernstpre 2:92c25cb669f4 134 }
altb2 0:d2e117716219 135 bool dum = mk.P2X(data.sens_phi,data.est_xy); // calculate actual xy-values, uncomment this if there are timing issues
altb2 0:d2e117716219 136 //current_path->get_x_v(glob_ti.read(),&phi_des,&v_des);
ernstpre 2:92c25cb669f4 137 } // else(..)
altb2 0:d2e117716219 138 laser_on = data.laser_on;
altb2 0:d2e117716219 139 i_enable = big_button;
ernstpre 2:92c25cb669f4 140 }// endof the main loop
altb2 0:d2e117716219 141 }
altb2 0:d2e117716219 142
ernstpre 2:92c25cb669f4 143 void ControllerLoop::sendSignal()
ernstpre 2:92c25cb669f4 144 {
altb2 0:d2e117716219 145 thread.flags_set(threadFlag);
altb2 0:d2e117716219 146 }
altb2 0:d2e117716219 147 void ControllerLoop::start_loop(void)
altb2 0:d2e117716219 148 {
altb2 0:d2e117716219 149 thread.start(callback(this, &ControllerLoop::loop));
altb2 0:d2e117716219 150 ticker.attach(callback(this, &ControllerLoop::sendSignal), Ts);
altb2 0:d2e117716219 151 }
altb2 0:d2e117716219 152
altb2 0:d2e117716219 153 float ControllerLoop::pos_cntrl(float d_phi)
altb2 0:d2e117716219 154 {
ernstpre 2:92c25cb669f4 155
ernstpre 2:92c25cb669f4 156 // write position controller here
ernstpre 2:92c25cb669f4 157 return 0.0;
ernstpre 2:92c25cb669f4 158 }
altb2 0:d2e117716219 159
altb2 0:d2e117716219 160 void ControllerLoop::init_controllers(void)
altb2 0:d2e117716219 161 {
altb2 0:d2e117716219 162 // set values for your velocity and position controller here!
ernstpre 2:92c25cb669f4 163
ernstpre 2:92c25cb669f4 164
altb2 0:d2e117716219 165 }
altb2 0:d2e117716219 166 // find_index: move axis slowly to detect the zero-pulse
altb2 0:d2e117716219 167 void ControllerLoop::find_index(void)
altb2 0:d2e117716219 168 {
altb2 0:d2e117716219 169 // use a simple P-controller to get system spinning, add a constant current to overcome friction
altb2 0:d2e117716219 170 float Kp = 0.005;
altb2 0:d2e117716219 171 float i1 = 0.2f + Kp*(50.0f - data.sens_Vphi[0]);
altb2 0:d2e117716219 172 float i2 = 0.2f + Kp*(50.0f - data.sens_Vphi[1]) ;
altb2 0:d2e117716219 173 i_des1.write(i2u(i1));
altb2 0:d2e117716219 174 i_des2.write(i2u(i2));
ernstpre 2:92c25cb669f4 175 }