File content as of revision 0:9bd4730782f9:

#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);
        }
             
    }
}