Diff: main.cpp

Revision:

0:9bd4730782f9

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sat Feb 17 11:05:43 2018 +0000
@@ -0,0 +1,93 @@
+#include "mbed.h"
+#include "MotorShieldIHM12A1.h"
+#include "hcsr04.h"
+
+#define  SPEED 50
+float DIR; 
+float mediaB = 0;
+float mediaN = 0 ;
+float MediaT = 0;
+
+#define Kp 50
+#define offset 0
+#define C 5
+
+HCSR04 sensor(D10, D11);
+// Serial
+Serial pc(USBTX, USBRX);
+//PwmOut mypwm(A0);
+ 
+MotorShieldIHM12A1 ms;
+AnalogIn lineaD(A5);
+DigitalIn pulsante(PC_13);
+
+int main()
+{
+    pc.baud(9600);
+    pc.printf("Here I am!\r\n");
+    //ms.MotorStart();
+    printf("start \r\n");
+    // Main loop
+    printf ("Val \r\n ");
+    
+    
+    
+    
+    
+    while (pulsante) 
+    {
+        wait (0.1);
+    }
+    wait(0.1);
+    
+    for (int i=0; i<5; i++)
+    {
+        mediaB += lineaD.read();
+        wait(0.1);
+    }
+    
+    mediaB /= C;
+    printf ("%f \n\r", mediaB);
+    printf("Bianco \r\n");
+    
+    while (pulsante) 
+    {
+        wait (0.1);
+    }
+    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);
+        }
+             
+    }
+}
\ No newline at end of file