Diff: main.cpp

Revision:

9:2a16e3930e1d

Parent:

8:41752698aa35

Child:

10:2a1abbd2c84c

--- a/main.cpp	Sat May 23 16:15:40 2020 +0000
+++ b/main.cpp	Sat May 23 17:37:14 2020 +0000
@@ -7,7 +7,7 @@
 
 #include "TB6549.h"
 
-#define dt0 0.01            // pas d'integration
+#define dt 0.01            // pas d'integration
 #define FDC_PLUS 0.9       // fin de course +
 #define FDC_MOINS 0.1      // din de course -
 #define ZM 2               // zone morte +/- zm
@@ -21,7 +21,7 @@
 LSM9DS1 DOF(PB_9, PB_8, 0xD4, 0x38);
 Thread thread (osPriorityAboveNormal);
 EventQueue queue;
-
+Mutex pr;
 AnalogIn verin(PC_3);  // lecture pos verin
 AnalogOut ana (PA_5);   // pour debug analogique ISR
 Motor motor(PB_4,PA_1,PA_4,PC_7);
@@ -52,7 +52,6 @@
 double angle_gyroy_f_pred=0.0f;
 double angle_gyroy_f=0.0f;
 //
-double dt=dt0 ; // pas reel
 double angle_final;
 //
 void calcul(void)
@@ -63,8 +62,8 @@
     a0=1./(1+(2*RC/dt));   //calcul du coefficient a du filtre
     b0=(1-(2.*RC/dt))*a0;    //calcul du coefficient b du filtre
     a1=a0*RC*1.0;           //calcul du coefficient a du filtre
-    dt=tt.read_us()/1000000.0;
-    tt.reset();
+    //dt=tt.read_us()/1000000.0;
+    //tt.reset();
     DOF.readAccel();
     DOF.readGyro();
 #if viti
@@ -95,42 +94,31 @@
 
 int main()
 {
-    dt=dt0;
-    tt.start();
+    //pc.printf("Hello World 1\r\n");
+    //dt=dt0;
+    //tt.start();
     wait(1);
-    //DOF.calibration();
     DOF.begin();
     wait(1);
     DOF.calibration();
     wait(1);
-    tt.reset();
+    //tt.reset();
     thread.start(callback(&queue,&EventQueue::dispatch_forever));
-    tic.attach(queue.event(calcul),0.01);
-
-
-
+    tic.attach(queue.event(&calcul),dt);
     while(1) {
-
-
         ana=0.6;
         //moteur
         float x=verin.read();
         float s=0.0;
-        /*
-        // sur table
-        if((angle_final>2)&&(x>0.1)) s=0.6;
-        else if((angle_final<-2)&&(x<0.9)) s=-0.6;
-        else motor.speed(0.0);
-        */
-        //sur site
         if((angle_final<-ZM)&&(x>FDC_MOINS)) s=SP;
         else if((angle_final>ZM)&&(x<FDC_PLUS)) s=-SP;
         else motor.speed(0.0);
         //
         motor.speed(s);
-        pc.printf("$%6.2f %6.2f %6.2f %6.2f %6.2f %6.2f %6.2f %f;\r\n",ang,angle_acce_f,angle_gyroy_f,angle_final,gyroy,x,s,dt);
+        //pr.lock();
+        pc.printf("$%6.2f %6.2f %6.2f %6.2f %6.2f %6.2f %6.2f;\r\n",ang,angle_acce_f,angle_gyroy_f,angle_final,gyroy,x,s);
+        //pr.unlock();
         ana=0.0;
-
         wait(0.095);
 
     }