![](/media/cache/profiles/altb_JGUSAOG.jpg.50x50_q85.jpg)
corrections altb2
Diff: ControllerLoop.cpp
- Revision:
- 2:92c25cb669f4
- Parent:
- 1:25a2b47ca291
- Child:
- 3:8aa09988fa58
--- a/ControllerLoop.cpp Thu Aug 05 08:27:51 2021 +0000 +++ b/ControllerLoop.cpp Tue Aug 24 08:51:13 2021 +0000 @@ -11,101 +11,137 @@ ti.reset(); ti.start(); data.laser_on = false; - } +} // decontructor for controller loop ControllerLoop::~ControllerLoop() {} // ---------------------------------------------------------------------------- // this is the main loop called every Ts with high priority -void ControllerLoop::loop(void){ - float w01=2*3.1415927 * 8; +void ControllerLoop::loop(void) +{ + float w01=2*3.1415927 * 2; float xy[2]; float exc = 0; PID_Cntrl v_cntrl_1(0.0153f, 3.06,0,0,Ts,-0.8,0.8); PID_Cntrl v_cntrl_2(0.0153f, 3.06,0,0,Ts,-0.8,0.8); - while(1) - { + + bool stop_rec = false; + int k=0; + float Logg[2000][4]; //float datal[2000][6]; + //int vel1 = 5; + //int vel2 =10; + printf("Starting Controller \r\n"); + 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_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) - { + if(!is_initialized) { find_index(); - if(index1.positionAtIndexPulse != 0 && index2.positionAtIndexPulse != 0) + if(index1.positionAtIndexPulse != 0 && index2.positionAtIndexPulse != 0) is_initialized=true; + } else { + if(k==0) + { + printf("Starting else loop \r\n"); } - else - { // float Kp = 0.005; // data.i_des[0] = 0.1f + Kp*(exc+50.0f - data.sens_Vphi[0]); // ------------------------ do the control first - // calculate desired currents here, you can do "anything" here, + // 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 - //float e1 = 50 - data.sens_Vphi[0]; - //float e2 = 50 - data.sens_Vphi[1]; - //float v_des1 = exc; - //float v_des2 = 0; - float phi1_des = 0.025f*sinf(2.0f* 3.14159f*2.0f*ti.read()); - float phi2_des = 0.025f*cosf(2.0f* 3.14159f*2.0f*ti.read()); - float Kv = 123; - float v_des1 = Kv*(phi1_des - data.sens_phi[0]); - float v_des2 = Kv*(phi2_des - data.sens_phi[1]); - data.i_des[0] = v_cntrl_1(v_des1 - data.sens_Vphi[0]); - data.i_des[1] = v_cntrl_2(v_des2 - data.sens_Vphi[1]); - - //data.i_des[1] =0.0; - + //float e1 = 50 - data.sens_Vphi[0]; + //float e2 = 50 - data.sens_Vphi[1]; + //float v_des1 = exc; + //float v_des2 = 0; + float phi1_des = 0.3f*sinf(2.0f* 3.14159f*2.0f*ti.read()); + float phi2_des = 0.3f*cosf(2.0f* 3.14159f*2.0f*ti.read()); + float Kv = 123; + float v_des1 = Kv*(phi1_des - data.sens_phi[0]); + float v_des2 = Kv*(phi2_des - data.sens_phi[1]); + data.i_des[0] = v_cntrl_1(v_des1 - data.sens_Vphi[0]); + data.i_des[1] = v_cntrl_2(v_des2 - data.sens_Vphi[1]); + + //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]); exc = myGPA(v_des1, data.sens_phi[0]); - + // + + /*if(k%10000==0) { + printf("yes \n"); + //printf("c1: %d c2: %d i2: %f\r\n",counts1,counts2,i2); + //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]); + }*/ + + if(k==200 && !stop_rec) + { + stop_rec = true; + k=0; + + for(int k1=0; k1<2000; k1++) + { + for(int k2=0; k2<4; k2++) + { + //printf("k1 = %d k2 = %d \r\n", k1, k2); + printf("%3.4f ",Logg[k1][k2]); + } + printf("\r\n"); + } + } + + if(k<2000 && !stop_rec) + { + Logg[k][0]=data.sens_phi[0]; + Logg[k][1]=data.sens_phi[1]; + Logg[k][2]=data.i_des[0]; + Logg[k][3]=data.i_des[1]; + } + k++; + // + // now do trafos etc - if(mk.external_control) // get desired values from external source (GUI) - { + 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 - { + // 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) - { + } 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 - { + } 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(..) + } // else(..) laser_on = data.laser_on; i_enable = big_button; - }// endof the main loop + }// endof the main loop } -void ControllerLoop::sendSignal() { +void ControllerLoop::sendSignal() +{ thread.flags_set(threadFlag); } void ControllerLoop::start_loop(void) @@ -116,16 +152,16 @@ float ControllerLoop::pos_cntrl(float d_phi) { - - // write position controller here - return 0.0; - } + + // 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) @@ -136,4 +172,4 @@ 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 +} \ No newline at end of file