Diff: ControllerLoop.cpp

Revision:

3:8aa09988fa58

Parent:

2:92c25cb669f4

diff -r 92c25cb669f4 -r 8aa09988fa58 ControllerLoop.cpp
--- a/ControllerLoop.cpp	Tue Aug 24 08:51:13 2021 +0000
+++ b/ControllerLoop.cpp	Tue Aug 24 12:34:00 2021 +0000
@@ -7,7 +7,7 @@
     this->Ts = Ts;
     diff1.reset(0.0f,0);
     diff2.reset(0.0f,0);
-    is_initialized = false;
+    is_initialized = true;
     ti.reset();
     ti.start();
     data.laser_on = false;
@@ -28,7 +28,7 @@
 
     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");
@@ -88,12 +88,11 @@
                 //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) 
+            if(k==N && !stop_rec) 
             {
                 stop_rec = true;
-                k=0;
-
-                for(int k1=0; k1<2000; k1++) 
+                
+                for(int k1=0; k1<N; k1++) 
                 {
                     for(int k2=0; k2<4; k2++) 
                     {
@@ -104,7 +103,7 @@
                 }
             }
 
-            if(k<2000 && !stop_rec)
+            if(k<N && !stop_rec)
             {
                 Logg[k][0]=data.sens_phi[0];
                 Logg[k][1]=data.sens_phi[1];