Diff: main.cpp

Revision:

1:0226a8d3c0d3

Parent:

0:9bd4730782f9

--- a/main.cpp	Sat Feb 17 11:05:43 2018 +0000
+++ b/main.cpp	Sat Feb 17 11:27:05 2018 +0000
@@ -2,20 +2,18 @@
 #include "MotorShieldIHM12A1.h"
 #include "hcsr04.h"
 
-#define  SPEED 50
+#define SPEED   40
+#define Kp      50
+#define offset  0
+#define sample  5
+
 float DIR; 
 float mediaB = 0;
 float mediaN = 0 ;
-float MediaT = 0;
-
-#define Kp 50
-#define offset 0
-#define C 5
+float mediaT = 0;
 
 HCSR04 sensor(D10, D11);
-// Serial
 Serial pc(USBTX, USBRX);
-//PwmOut mypwm(A0);
  
 MotorShieldIHM12A1 ms;
 AnalogIn lineaD(A5);
@@ -25,15 +23,9 @@
 {
     pc.baud(9600);
     pc.printf("Here I am!\r\n");
-    //ms.MotorStart();
-    printf("start \r\n");
-    // Main loop
-    printf ("Val \r\n ");
+    printf("start Seguilinea.os\r\n");
     
-    
-    
-    
-    
+    printf ("Robot su Bianco\r\n ");
     while (pulsante) 
     {
         wait (0.1);
@@ -46,9 +38,26 @@
         wait(0.1);
     }
     
-    mediaB /= C;
-    printf ("%f \n\r", mediaB);
-    printf("Bianco \r\n");
+    mediaB /= sample;
+    printf("Bianco: %f\r\n",mediaB);
+    
+    printf ("Robot su Nero\r\n ");
+    while (pulsante) 
+    {
+        wait (0.1);
+    }
+    wait(0.1);
+    
+    for (int i=0; i<sample; i++) 
+    {
+        mediaN += lineaD.read();
+        wait(0.1);
+    }
+    mediaN /= sample;
+    printf("Nero: %f\r\n",mediaN);
+    
+    mediaT =(mediaB+mediaN) / 2;
+    printf("Offset: %f\r\n",mediaT);
     
     while (pulsante) 
     {
@@ -56,37 +65,25 @@
     }
     wait(0.1);
     
-    for (int i=0; i<C; i++) 
-    {
-        mediaN += lineaD.read();
-        wait(0.1);
-    }
-    mediaN /= C;
-        printf ("%f \n\r", mediaB);
-    printf("Nero \r\n");
-    MediaT =(mediaB+mediaN) / 2;
     
-    while (pulsante) {
-        wait (0.1);
-    }
-    wait(0.1);
     while(true) 
     {
-    // Avvia un impulso della durata di 10us sul pin di trigger
-    sensor.start();
-    
-    // Aspetta prima della prossima lettura
-    wait_ms(300); 
-    
-    // Stampa sulla seriale la misura della distanza in cm
-    pc.printf("%dcm\r\n", sensor.get_dist_cm());
-    
-     if (sensor.get_dist_cm() > 20) {
-    
-            ms.turn((lineaD.read()-MediaT)*Kp,40); 
-    } 
-    else {
-        ms.turn(0,0);
+        // Avvia un impulso della durata di 10us sul pin di trigger
+        sensor.start();
+        
+        // Aspetta prima della prossima lettura
+        wait_ms(300); 
+        
+        // Stampa sulla seriale la misura della distanza in cm
+        pc.printf("%dcm\r\n", sensor.get_dist_cm());
+        
+        if (sensor.get_dist_cm() > 20)
+        {
+            ms.turn((lineaD.read()-mediaT)*Kp,SPEED); 
+        }
+        else
+        {
+            ms.turn(0,0);
         }
              
     }