Preston Ernst
-data logging revision
Diff: ControllerLoop.cpp
- Revision:
- 2:92c25cb669f4
- Parent:
- 1:25a2b47ca291
--- 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