Diff: main.cpp

Revision:

2:8817ad0d0a78

Parent:

1:7d94c1b86ad6

Child:

3:f56ec086a122

--- a/main.cpp	Mon Apr 15 11:44:17 2019 +0000
+++ b/main.cpp	Thu May 02 10:25:13 2019 +0000
@@ -9,19 +9,22 @@
 Serial pc(USBTX, USBRX);
 
 Timer t2;
-double  distance ;
+
 double rc;
 
 // Definition des variables : codeur
-InterruptIn ComptIncr(PA_8);
+InterruptIn ComptIncr(D7);
 
 Timer t1;
 
 float tps1;
 float tps2;
-float k;
+float tps3;
+float dt;
 int a;
-float b;
+double v;
+float dist;
+float dist_securite;
 
 // Definition des fonctions : moteur
 
@@ -33,15 +36,25 @@
 void front_descendant()
 {
     t2.stop();
-    double distance = 330*t2.read()/2*1.06;
-    pc.printf("Distance = %lf \r\n", distance);
+    tps3=t2.read();
+    dist=174.9*tps3;
+ //   pc.printf("dist = %lf \r\n", dist);
     t2.reset();
-    if (distance<=0.10){ rc = 0;}
-    if (distance<=0.20 && distance>0.10){ rc = 0.5;}
-    if (distance >=0.20) { rc = 1; }
+  //  if (dist == 0.0000){ rc = 1;}
+   /* if (dist<=v*2){ rc = 0;}
+    if (dist<= v*3 && dist>v*2){ rc = 0.2;}
+    if (dist<= v*4 && dist>v*3){ rc = 0.4;}
+    if (dist<= v*5 && dist>v*4){ rc = 0.6;}
+    if (dist<= v*6 && dist>v*5){ rc = 0.8;}
+    if (dist >= v*6) { rc = 1; }*/
+    //if (dist<= dist_securite){rc = rc-rc/200;}
+    //if (dist> dist_securite){rc = rc+(rc+1)/200;}
+    //if (dist<=0.1) {rc = 0;}
+    rc= 1;
     moteur2.write(rc);
     
-    pc.printf("RC = %lf \r\n", rc);
+  //  pc.printf("RC = %lf \r\n", rc);
+    
 } 
 
 // Definition des fonctions : codeur
@@ -50,15 +63,17 @@
 
 void vitesse(){
     if (a == 0){
-        tps1 = t1.read();
-        k = 0;
-        }    
-        a++;
-    if (a == 81) {
+        tps1 = t1.read(); 
+        }
+    a++;
+    if (a == 8) {
         tps2 = t1.read();
-        k = tps2 - tps1;
-      // printf("Intervalle de temps =%f\n",k);
+        dt = tps2 - tps1;
+        v = 0.0201056/dt; // (8/25)*2*pi*4.5 cm de rayon
+       // pc.printf("v = %lf\n", v);
+       // pc.printf("dt = %f\n", 1/dt);
         a = 0;
+        dist_securite = 6*v;
         //printf("Vitesse = %f tours/s\n", 10/t.read());
     }  
 }    
@@ -80,8 +95,7 @@
     
     // Initialisation Codeur
     a = 0;
-    k = 0;
-    b = 0;
+    dt = 0;
     t1.start();
 
     ComptIncr.rise(&vitesse);
@@ -91,13 +105,16 @@
     {
         // Partie moteur
         Trig = 1;
+        pc.printf("dist = %f \r\n", dist);
+        pc.printf("RC = %lf \r\n", rc);
+        pc.printf("v = %lf\n", v);
+
         wait(0.001);
         Trig =0 ; 
         wait(0.001);
 
     
         // Partie codeur
-        b = t1.read();
         //printf("Compteur = %d\n", a);
         //printf("Temps 1 = %f\n", tps1);
         //printf("Temps 2 = %f\n", tps2);