Ruprecht Altenburger / Mbed OS simple_program

Simple program for introduction of mirror actuator.

ControllerLoop.cpp@11:d43f8b421d6d, 2021-04-28 (annotated)

Committer:
altb2
Date:
Wed Apr 28 12:51:02 2021 +0000
Revision:
11:d43f8b421d6d
Parent:
10:bfacffec199a
Child:
13:1bf960928a93
interm2

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 10:bfacffec199a 4 // contructor for controller loop
altb2 10:bfacffec199a 5 ControllerLoop::ControllerLoop(float Ts) : thread(osPriorityHigh,4096), dout1(PB_9)
altb2 5:768e10f6d372 6 {
altb2 6:9ebeffe446e4 7 this->Ts = Ts;
altb2 5:768e10f6d372 8 diff1.reset(0.0f,0);
altb2 5:768e10f6d372 9 diff2.reset(0.0f,0);
altb2 6:9ebeffe446e4 10 is_initialized = false;
altb2 6:9ebeffe446e4 11 ti.reset();
altb2 6:9ebeffe446e4 12 ti.start();
altb2 7:942fd77d5e19 13 data.laser_on = false;
altb2 5:768e10f6d372 14 }
altb2 5:768e10f6d372 15
altb2 10:bfacffec199a 16 // decontructor for controller loop
altb2 5:768e10f6d372 17 ControllerLoop::~ControllerLoop() {}
altb2 5:768e10f6d372 18
altb2 10:bfacffec199a 19 // ----------------------------------------------------------------------------
altb2 10:bfacffec199a 20 // this is the main loop called every Ts with high priority
altb2 5:768e10f6d372 21 void ControllerLoop::loop(void){
altb2 8:49ac75c42da0 22 float w01=2*3.1415927 * 8;
altb2 8:49ac75c42da0 23 float xy[2];
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 10:bfacffec199a 28 short c1 = counter1 - index1.positionAtIndexPulse - mk.inc_offset[0]- mk.inc_additional_offset[0]; // get counts from Encoder
altb2 10:bfacffec199a 29 short c2 = counter2 - index2.positionAtIndexPulse - mk.inc_offset[1]- mk.inc_additional_offset[1]; // get counts from Encoder
altb2 10:bfacffec199a 30 data.sens_phi[0] = uw2pi1(2.0f*3.1415927f/4000.0f*(float)c1);
altb2 10:bfacffec199a 31 data.sens_Vphi[0] = diff1(c1); // motor velocity
altb2 10:bfacffec199a 32 data.sens_phi[1] = uw2pi2(2.0f*3.1415927f/4000.0f*(float)c2);
altb2 10:bfacffec199a 33 data.sens_Vphi[1] = diff2(c2); // motor velocity
altb2 10:bfacffec199a 34 // -------------------------------------------------------------
altb2 10:bfacffec199a 35 // at very beginning: move system slowly to find the zero pulse
altb2 10:bfacffec199a 36 // set "if(0)" if you like to ommit at beginning
altb2 7:942fd77d5e19 37 if(!is_initialized)
altb2 6:9ebeffe446e4 38 {
altb2 6:9ebeffe446e4 39 find_index();
altb2 6:9ebeffe446e4 40 if(index1.positionAtIndexPulse != 0 && index2.positionAtIndexPulse != 0)
altb2 6:9ebeffe446e4 41 is_initialized=true;
altb2 5:768e10f6d372 42 }
altb2 6:9ebeffe446e4 43 else
altb2 6:9ebeffe446e4 44 {
altb2 7:942fd77d5e19 45 // ------------------------ do the control first
altb2 11:d43f8b421d6d 46
altb2 11:d43f8b421d6d 47 // calculate desired currents here, you can do "anything" here,
altb2 11:d43f8b421d6d 48 // if you like to refer to values e.g. from the gui or from the trafo,
altb2 11:d43f8b421d6d 49 // please use data.xxx values, they are calculated 30 lines below
altb2 11:d43f8b421d6d 50
altb2 11:d43f8b421d6d 51 data.i_des[0] = data.i_des[1] =0.0;
altb2 11:d43f8b421d6d 52
altb2 7:942fd77d5e19 53 // ------------------------ write outputs
altb2 7:942fd77d5e19 54 i_des1.write(i2u(data.i_des[0]));
altb2 7:942fd77d5e19 55 i_des2.write(i2u(data.i_des[1]));
altb2 11:d43f8b421d6d 56 // GPA: if you want to use the GPA, uncomment and improve following line:
altb2 11:d43f8b421d6d 57 // exc = myGPA(data.i_des[0],data.sens_Vphi[0]);
altb2 11:d43f8b421d6d 58
altb2 7:942fd77d5e19 59 // now do trafos etc
altb2 7:942fd77d5e19 60
altb2 7:942fd77d5e19 61 if(mk.external_control) // get desired values from external source (GUI)
altb2 7:942fd77d5e19 62 {
altb2 7:942fd77d5e19 63 if(mk.trafo_is_on) // use desired xy values from xternal source and transform
altb2 7:942fd77d5e19 64 // otherwise external source delivers phi1, phi2 values directly
altb2 7:942fd77d5e19 65 {
altb2 7:942fd77d5e19 66 bool dum = mk.X2P(data.cntrl_xy_des,data.cntrl_phi_des);
altb2 7:942fd77d5e19 67 }
altb2 7:942fd77d5e19 68 }
altb2 11:d43f8b421d6d 69 else // this is called, when desired values are calculated here internally (e.g. pathplanner)
altb2 7:942fd77d5e19 70 {
altb2 7:942fd77d5e19 71 if(mk.trafo_is_on)
altb2 7:942fd77d5e19 72 {
altb2 11:d43f8b421d6d 73 data.cntrl_xy_des[0] = 30.0f*cosf(w01*glob_ti.read()); // make a circle in xy-co-ordinates
altb2 8:49ac75c42da0 74 data.cntrl_xy_des[1] = 30.0f*sinf(w01*glob_ti.read());
altb2 7:942fd77d5e19 75 bool dum = mk.X2P(data.cntrl_xy_des,data.cntrl_phi_des);
altb2 7:942fd77d5e19 76 }
altb2 7:942fd77d5e19 77 else
altb2 7:942fd77d5e19 78 {
altb2 11:d43f8b421d6d 79 data.cntrl_phi_des[0] = .250f*cosf(w01*glob_ti.read()); // make some harmonic movements directly on phi1/phi2
altb2 8:49ac75c42da0 80 data.cntrl_phi_des[1] = .250f*sinf(w01*glob_ti.read());
altb2 7:942fd77d5e19 81 }
altb2 7:942fd77d5e19 82 }
altb2 11:d43f8b421d6d 83 bool dum = mk.P2X(data.sens_phi,data.est_xy); // calculate actual xy-values, uncomment this if there are timing issues
altb2 6:9ebeffe446e4 84 //current_path->get_x_v(glob_ti.read(),&phi_des,&v_des);
altb2 11:d43f8b421d6d 85 } // else(..)
altb2 8:49ac75c42da0 86 laser_on = data.laser_on;
altb2 8:49ac75c42da0 87 i_enable = big_button;
altb2 11:d43f8b421d6d 88 }// endof the main loop
altb2 5:768e10f6d372 89 }
altb2 5:768e10f6d372 90
altb2 5:768e10f6d372 91 void ControllerLoop::sendSignal() {
altb2 5:768e10f6d372 92 thread.flags_set(threadFlag);
altb2 5:768e10f6d372 93 }
altb2 6:9ebeffe446e4 94 void ControllerLoop::start_loop(void)
altb2 6:9ebeffe446e4 95 {
altb2 6:9ebeffe446e4 96 thread.start(callback(this, &ControllerLoop::loop));
altb2 6:9ebeffe446e4 97 ticker.attach(callback(this, &ControllerLoop::sendSignal), Ts);
altb2 6:9ebeffe446e4 98 }
altb2 7:942fd77d5e19 99
altb2 7:942fd77d5e19 100 float ControllerLoop::pos_cntrl(float d_phi)
altb2 7:942fd77d5e19 101 {
altb2 11:d43f8b421d6d 102
altb2 11:d43f8b421d6d 103 // write position controller here
altb2 11:d43f8b421d6d 104 return 0.0;
altb2 7:942fd77d5e19 105 }
altb2 7:942fd77d5e19 106
altb2 6:9ebeffe446e4 107 void ControllerLoop::init_controllers(void)
altb2 6:9ebeffe446e4 108 {
altb2 11:d43f8b421d6d 109 // set values for your velocity and position controller here!
altb2 11:d43f8b421d6d 110
altb2 11:d43f8b421d6d 111
altb2 6:9ebeffe446e4 112 }
altb2 10:bfacffec199a 113 // find_index: move axis slowly to detect the zero-pulse
altb2 6:9ebeffe446e4 114 void ControllerLoop::find_index(void)
altb2 6:9ebeffe446e4 115 {
altb2 10:bfacffec199a 116 // use a simple P-controller to get system spinning, add a constant current to overcome friction
altb2 6:9ebeffe446e4 117 float Kp = 0.005;
altb2 11:d43f8b421d6d 118 float i1 = 0.2f + Kp*(50.0f - data.sens_Vphi[0]);
altb2 11:d43f8b421d6d 119 float i2 = 0.2f + Kp*(50.0f - data.sens_Vphi[1]) ;
altb2 7:942fd77d5e19 120 i_des1.write(i2u(i1));
altb2 6:9ebeffe446e4 121 i_des2.write(i2u(i2));
altb2 6:9ebeffe446e4 122 }