DuckietownHS_Marcianise / Mbed 2 deprecated PLT_Seguilinea_Kp_Taratura

Seguilinea KP Taratura Ultrasuoni di Paolo Giovanni

Dependencies:   HCSR04 X_NUCLEO_IHM12A1 mbed

Fork of Ilfunzionante by Marcianise NAS

main.cpp@0:9bd4730782f9, 2018-02-17 (annotated)

Committer:
PaoloGiovanni99
Date:
Sat Feb 17 11:05:43 2018 +0000
Revision:
0:9bd4730782f9
Child:
1:0226a8d3c0d3
seguilinea funzionante Marcianise

Who changed what in which revision?

UserRevisionLine numberNew contents of line
PaoloGiovanni99 0:9bd4730782f9 1 #include "mbed.h"
PaoloGiovanni99 0:9bd4730782f9 2 #include "MotorShieldIHM12A1.h"
PaoloGiovanni99 0:9bd4730782f9 3 #include "hcsr04.h"
PaoloGiovanni99 0:9bd4730782f9 4
PaoloGiovanni99 0:9bd4730782f9 5 #define SPEED 50
PaoloGiovanni99 0:9bd4730782f9 6 float DIR;
PaoloGiovanni99 0:9bd4730782f9 7 float mediaB = 0;
PaoloGiovanni99 0:9bd4730782f9 8 float mediaN = 0 ;
PaoloGiovanni99 0:9bd4730782f9 9 float MediaT = 0;
PaoloGiovanni99 0:9bd4730782f9 10
PaoloGiovanni99 0:9bd4730782f9 11 #define Kp 50
PaoloGiovanni99 0:9bd4730782f9 12 #define offset 0
PaoloGiovanni99 0:9bd4730782f9 13 #define C 5
PaoloGiovanni99 0:9bd4730782f9 14
PaoloGiovanni99 0:9bd4730782f9 15 HCSR04 sensor(D10, D11);
PaoloGiovanni99 0:9bd4730782f9 16 // Serial
PaoloGiovanni99 0:9bd4730782f9 17 Serial pc(USBTX, USBRX);
PaoloGiovanni99 0:9bd4730782f9 18 //PwmOut mypwm(A0);
PaoloGiovanni99 0:9bd4730782f9 19
PaoloGiovanni99 0:9bd4730782f9 20 MotorShieldIHM12A1 ms;
PaoloGiovanni99 0:9bd4730782f9 21 AnalogIn lineaD(A5);
PaoloGiovanni99 0:9bd4730782f9 22 DigitalIn pulsante(PC_13);
PaoloGiovanni99 0:9bd4730782f9 23
PaoloGiovanni99 0:9bd4730782f9 24 int main()
PaoloGiovanni99 0:9bd4730782f9 25 {
PaoloGiovanni99 0:9bd4730782f9 26 pc.baud(9600);
PaoloGiovanni99 0:9bd4730782f9 27 pc.printf("Here I am!\r\n");
PaoloGiovanni99 0:9bd4730782f9 28 //ms.MotorStart();
PaoloGiovanni99 0:9bd4730782f9 29 printf("start \r\n");
PaoloGiovanni99 0:9bd4730782f9 30 // Main loop
PaoloGiovanni99 0:9bd4730782f9 31 printf ("Val \r\n ");
PaoloGiovanni99 0:9bd4730782f9 32
PaoloGiovanni99 0:9bd4730782f9 33
PaoloGiovanni99 0:9bd4730782f9 34
PaoloGiovanni99 0:9bd4730782f9 35
PaoloGiovanni99 0:9bd4730782f9 36
PaoloGiovanni99 0:9bd4730782f9 37 while (pulsante)
PaoloGiovanni99 0:9bd4730782f9 38 {
PaoloGiovanni99 0:9bd4730782f9 39 wait (0.1);
PaoloGiovanni99 0:9bd4730782f9 40 }
PaoloGiovanni99 0:9bd4730782f9 41 wait(0.1);
PaoloGiovanni99 0:9bd4730782f9 42
PaoloGiovanni99 0:9bd4730782f9 43 for (int i=0; i<5; i++)
PaoloGiovanni99 0:9bd4730782f9 44 {
PaoloGiovanni99 0:9bd4730782f9 45 mediaB += lineaD.read();
PaoloGiovanni99 0:9bd4730782f9 46 wait(0.1);
PaoloGiovanni99 0:9bd4730782f9 47 }
PaoloGiovanni99 0:9bd4730782f9 48
PaoloGiovanni99 0:9bd4730782f9 49 mediaB /= C;
PaoloGiovanni99 0:9bd4730782f9 50 printf ("%f \n\r", mediaB);
PaoloGiovanni99 0:9bd4730782f9 51 printf("Bianco \r\n");
PaoloGiovanni99 0:9bd4730782f9 52
PaoloGiovanni99 0:9bd4730782f9 53 while (pulsante)
PaoloGiovanni99 0:9bd4730782f9 54 {
PaoloGiovanni99 0:9bd4730782f9 55 wait (0.1);
PaoloGiovanni99 0:9bd4730782f9 56 }
PaoloGiovanni99 0:9bd4730782f9 57 wait(0.1);
PaoloGiovanni99 0:9bd4730782f9 58
PaoloGiovanni99 0:9bd4730782f9 59 for (int i=0; i<C; i++)
PaoloGiovanni99 0:9bd4730782f9 60 {
PaoloGiovanni99 0:9bd4730782f9 61 mediaN += lineaD.read();
PaoloGiovanni99 0:9bd4730782f9 62 wait(0.1);
PaoloGiovanni99 0:9bd4730782f9 63 }
PaoloGiovanni99 0:9bd4730782f9 64 mediaN /= C;
PaoloGiovanni99 0:9bd4730782f9 65 printf ("%f \n\r", mediaB);
PaoloGiovanni99 0:9bd4730782f9 66 printf("Nero \r\n");
PaoloGiovanni99 0:9bd4730782f9 67 MediaT =(mediaB+mediaN) / 2;
PaoloGiovanni99 0:9bd4730782f9 68
PaoloGiovanni99 0:9bd4730782f9 69 while (pulsante) {
PaoloGiovanni99 0:9bd4730782f9 70 wait (0.1);
PaoloGiovanni99 0:9bd4730782f9 71 }
PaoloGiovanni99 0:9bd4730782f9 72 wait(0.1);
PaoloGiovanni99 0:9bd4730782f9 73 while(true)
PaoloGiovanni99 0:9bd4730782f9 74 {
PaoloGiovanni99 0:9bd4730782f9 75 // Avvia un impulso della durata di 10us sul pin di trigger
PaoloGiovanni99 0:9bd4730782f9 76 sensor.start();
PaoloGiovanni99 0:9bd4730782f9 77
PaoloGiovanni99 0:9bd4730782f9 78 // Aspetta prima della prossima lettura
PaoloGiovanni99 0:9bd4730782f9 79 wait_ms(300);
PaoloGiovanni99 0:9bd4730782f9 80
PaoloGiovanni99 0:9bd4730782f9 81 // Stampa sulla seriale la misura della distanza in cm
PaoloGiovanni99 0:9bd4730782f9 82 pc.printf("%dcm\r\n", sensor.get_dist_cm());
PaoloGiovanni99 0:9bd4730782f9 83
PaoloGiovanni99 0:9bd4730782f9 84 if (sensor.get_dist_cm() > 20) {
PaoloGiovanni99 0:9bd4730782f9 85
PaoloGiovanni99 0:9bd4730782f9 86 ms.turn((lineaD.read()-MediaT)*Kp,40);
PaoloGiovanni99 0:9bd4730782f9 87 }
PaoloGiovanni99 0:9bd4730782f9 88 else {
PaoloGiovanni99 0:9bd4730782f9 89 ms.turn(0,0);
PaoloGiovanni99 0:9bd4730782f9 90 }
PaoloGiovanni99 0:9bd4730782f9 91
PaoloGiovanni99 0:9bd4730782f9 92 }
PaoloGiovanni99 0:9bd4730782f9 93 }