Ruprecht Altenburger
/
simple_program
Simple program for introduction of mirror actuator.
Diff: ControllerLoop.cpp
- Revision:
- 13:1bf960928a93
- Parent:
- 11:d43f8b421d6d
- Child:
- 14:1be03d1c45c7
--- a/ControllerLoop.cpp Wed Apr 28 12:51:02 2021 +0000 +++ b/ControllerLoop.cpp Sat May 01 20:08:24 2021 +0000 @@ -2,16 +2,10 @@ using namespace std; // contructor for controller loop -ControllerLoop::ControllerLoop(float Ts) : thread(osPriorityHigh,4096), dout1(PB_9) +ControllerLoop::ControllerLoop(float Ts) : thread(osPriorityHigh,4096) { - this->Ts = Ts; - diff1.reset(0.0f,0); - diff2.reset(0.0f,0); - is_initialized = false; - ti.reset(); - ti.start(); - data.laser_on = false; - } + this->Ts = Ts; +} // decontructor for controller loop ControllerLoop::~ControllerLoop() {} @@ -19,72 +13,11 @@ // ---------------------------------------------------------------------------- // this is the main loop called every Ts with high priority void ControllerLoop::loop(void){ - float w01=2*3.1415927 * 8; - float xy[2]; while(1) { ThisThread::flags_wait_any(threadFlag); // THE LOOP ------------------------------------------------------------ - short c1 = counter1 - index1.positionAtIndexPulse - mk.inc_offset[0]- mk.inc_additional_offset[0]; // get counts from Encoder - short c2 = counter2 - index2.positionAtIndexPulse - mk.inc_offset[1]- mk.inc_additional_offset[1]; // get counts from Encoder - data.sens_phi[0] = uw2pi1(2.0f*3.1415927f/4000.0f*(float)c1); - data.sens_Vphi[0] = diff1(c1); // motor velocity - data.sens_phi[1] = uw2pi2(2.0f*3.1415927f/4000.0f*(float)c2); - data.sens_Vphi[1] = diff2(c2); // motor velocity - // ------------------------------------------------------------- - // at very beginning: move system slowly to find the zero pulse - // set "if(0)" if you like to ommit at beginning - if(!is_initialized) - { - find_index(); - if(index1.positionAtIndexPulse != 0 && index2.positionAtIndexPulse != 0) - is_initialized=true; - } - else - { - // ------------------------ do the control first - - // calculate desired currents here, you can do "anything" here, - // if you like to refer to values e.g. from the gui or from the trafo, - // please use data.xxx values, they are calculated 30 lines below - - data.i_des[0] = data.i_des[1] =0.0; - - // ------------------------ write outputs - i_des1.write(i2u(data.i_des[0])); - i_des2.write(i2u(data.i_des[1])); - // GPA: if you want to use the GPA, uncomment and improve following line: - // exc = myGPA(data.i_des[0],data.sens_Vphi[0]); - - // now do trafos etc - - if(mk.external_control) // get desired values from external source (GUI) - { - if(mk.trafo_is_on) // use desired xy values from xternal source and transform - // otherwise external source delivers phi1, phi2 values directly - { - bool dum = mk.X2P(data.cntrl_xy_des,data.cntrl_phi_des); - } - } - else // this is called, when desired values are calculated here internally (e.g. pathplanner) - { - if(mk.trafo_is_on) - { - data.cntrl_xy_des[0] = 30.0f*cosf(w01*glob_ti.read()); // make a circle in xy-co-ordinates - data.cntrl_xy_des[1] = 30.0f*sinf(w01*glob_ti.read()); - bool dum = mk.X2P(data.cntrl_xy_des,data.cntrl_phi_des); - } - else - { - data.cntrl_phi_des[0] = .250f*cosf(w01*glob_ti.read()); // make some harmonic movements directly on phi1/phi2 - data.cntrl_phi_des[1] = .250f*sinf(w01*glob_ti.read()); - } - } - bool dum = mk.P2X(data.sens_phi,data.est_xy); // calculate actual xy-values, uncomment this if there are timing issues - //current_path->get_x_v(glob_ti.read(),&phi_des,&v_des); - } // else(..) - laser_on = data.laser_on; - i_enable = big_button; + //printf("I am called!\r\n"); }// endof the main loop } @@ -97,26 +30,7 @@ ticker.attach(callback(this, &ControllerLoop::sendSignal), Ts); } -float ControllerLoop::pos_cntrl(float d_phi) -{ - - // write position controller here - return 0.0; - } - void ControllerLoop::init_controllers(void) { // set values for your velocity and position controller here! - - } -// find_index: move axis slowly to detect the zero-pulse -void ControllerLoop::find_index(void) -{ - // use a simple P-controller to get system spinning, add a constant current to overcome friction - float Kp = 0.005; - float i1 = 0.2f + Kp*(50.0f - data.sens_Vphi[0]); - float i2 = 0.2f + Kp*(50.0f - data.sens_Vphi[1]) ; - i_des1.write(i2u(i1)); - i_des2.write(i2u(i2)); - } \ No newline at end of file