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@1:0226a8d3c0d3, 2018-02-17 (annotated)

Committer:
FrancescoCaiazzo
Date:
Sat Feb 17 11:27:05 2018 +0000
Revision:
1:0226a8d3c0d3
Parent:
0:9bd4730782f9 
Seguilinea PLT Paolo Giovanni di 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
FrancescoCaiazzo 1:0226a8d3c0d3 5 #define SPEED 40
FrancescoCaiazzo 1:0226a8d3c0d3 6 #define Kp 50
FrancescoCaiazzo 1:0226a8d3c0d3 7 #define offset 0
FrancescoCaiazzo 1:0226a8d3c0d3 8 #define sample 5
FrancescoCaiazzo 1:0226a8d3c0d3 9
PaoloGiovanni99 0:9bd4730782f9 10 float DIR;
PaoloGiovanni99 0:9bd4730782f9 11 float mediaB = 0;
PaoloGiovanni99 0:9bd4730782f9 12 float mediaN = 0 ;
FrancescoCaiazzo 1:0226a8d3c0d3 13 float mediaT = 0;
PaoloGiovanni99 0:9bd4730782f9 14
PaoloGiovanni99 0:9bd4730782f9 15 HCSR04 sensor(D10, D11);
PaoloGiovanni99 0:9bd4730782f9 16 Serial pc(USBTX, USBRX);
PaoloGiovanni99 0:9bd4730782f9 17
PaoloGiovanni99 0:9bd4730782f9 18 MotorShieldIHM12A1 ms;
PaoloGiovanni99 0:9bd4730782f9 19 AnalogIn lineaD(A5);
PaoloGiovanni99 0:9bd4730782f9 20 DigitalIn pulsante(PC_13);
PaoloGiovanni99 0:9bd4730782f9 21
PaoloGiovanni99 0:9bd4730782f9 22 int main()
PaoloGiovanni99 0:9bd4730782f9 23 {
PaoloGiovanni99 0:9bd4730782f9 24 pc.baud(9600);
PaoloGiovanni99 0:9bd4730782f9 25 pc.printf("Here I am!\r\n");
FrancescoCaiazzo 1:0226a8d3c0d3 26 printf("start Seguilinea.os\r\n");
PaoloGiovanni99 0:9bd4730782f9 27
FrancescoCaiazzo 1:0226a8d3c0d3 28 printf ("Robot su Bianco\r\n ");
PaoloGiovanni99 0:9bd4730782f9 29 while (pulsante)
PaoloGiovanni99 0:9bd4730782f9 30 {
PaoloGiovanni99 0:9bd4730782f9 31 wait (0.1);
PaoloGiovanni99 0:9bd4730782f9 32 }
PaoloGiovanni99 0:9bd4730782f9 33 wait(0.1);
PaoloGiovanni99 0:9bd4730782f9 34
PaoloGiovanni99 0:9bd4730782f9 35 for (int i=0; i<5; i++)
PaoloGiovanni99 0:9bd4730782f9 36 {
PaoloGiovanni99 0:9bd4730782f9 37 mediaB += lineaD.read();
PaoloGiovanni99 0:9bd4730782f9 38 wait(0.1);
PaoloGiovanni99 0:9bd4730782f9 39 }
PaoloGiovanni99 0:9bd4730782f9 40
FrancescoCaiazzo 1:0226a8d3c0d3 41 mediaB /= sample;
FrancescoCaiazzo 1:0226a8d3c0d3 42 printf("Bianco: %f\r\n",mediaB);
FrancescoCaiazzo 1:0226a8d3c0d3 43
FrancescoCaiazzo 1:0226a8d3c0d3 44 printf ("Robot su Nero\r\n ");
FrancescoCaiazzo 1:0226a8d3c0d3 45 while (pulsante)
FrancescoCaiazzo 1:0226a8d3c0d3 46 {
FrancescoCaiazzo 1:0226a8d3c0d3 47 wait (0.1);
FrancescoCaiazzo 1:0226a8d3c0d3 48 }
FrancescoCaiazzo 1:0226a8d3c0d3 49 wait(0.1);
FrancescoCaiazzo 1:0226a8d3c0d3 50
FrancescoCaiazzo 1:0226a8d3c0d3 51 for (int i=0; i<sample; i++)
FrancescoCaiazzo 1:0226a8d3c0d3 52 {
FrancescoCaiazzo 1:0226a8d3c0d3 53 mediaN += lineaD.read();
FrancescoCaiazzo 1:0226a8d3c0d3 54 wait(0.1);
FrancescoCaiazzo 1:0226a8d3c0d3 55 }
FrancescoCaiazzo 1:0226a8d3c0d3 56 mediaN /= sample;
FrancescoCaiazzo 1:0226a8d3c0d3 57 printf("Nero: %f\r\n",mediaN);
FrancescoCaiazzo 1:0226a8d3c0d3 58
FrancescoCaiazzo 1:0226a8d3c0d3 59 mediaT =(mediaB+mediaN) / 2;
FrancescoCaiazzo 1:0226a8d3c0d3 60 printf("Offset: %f\r\n",mediaT);
PaoloGiovanni99 0:9bd4730782f9 61
PaoloGiovanni99 0:9bd4730782f9 62 while (pulsante)
PaoloGiovanni99 0:9bd4730782f9 63 {
PaoloGiovanni99 0:9bd4730782f9 64 wait (0.1);
PaoloGiovanni99 0:9bd4730782f9 65 }
PaoloGiovanni99 0:9bd4730782f9 66 wait(0.1);
PaoloGiovanni99 0:9bd4730782f9 67
PaoloGiovanni99 0:9bd4730782f9 68
PaoloGiovanni99 0:9bd4730782f9 69 while(true)
PaoloGiovanni99 0:9bd4730782f9 70 {
FrancescoCaiazzo 1:0226a8d3c0d3 71 // Avvia un impulso della durata di 10us sul pin di trigger
FrancescoCaiazzo 1:0226a8d3c0d3 72 sensor.start();
FrancescoCaiazzo 1:0226a8d3c0d3 73
FrancescoCaiazzo 1:0226a8d3c0d3 74 // Aspetta prima della prossima lettura
FrancescoCaiazzo 1:0226a8d3c0d3 75 wait_ms(300);
FrancescoCaiazzo 1:0226a8d3c0d3 76
FrancescoCaiazzo 1:0226a8d3c0d3 77 // Stampa sulla seriale la misura della distanza in cm
FrancescoCaiazzo 1:0226a8d3c0d3 78 pc.printf("%dcm\r\n", sensor.get_dist_cm());
FrancescoCaiazzo 1:0226a8d3c0d3 79
FrancescoCaiazzo 1:0226a8d3c0d3 80 if (sensor.get_dist_cm() > 20)
FrancescoCaiazzo 1:0226a8d3c0d3 81 {
FrancescoCaiazzo 1:0226a8d3c0d3 82 ms.turn((lineaD.read()-mediaT)*Kp,SPEED);
FrancescoCaiazzo 1:0226a8d3c0d3 83 }
FrancescoCaiazzo 1:0226a8d3c0d3 84 else
FrancescoCaiazzo 1:0226a8d3c0d3 85 {
FrancescoCaiazzo 1:0226a8d3c0d3 86 ms.turn(0,0);
PaoloGiovanni99 0:9bd4730782f9 87 }
PaoloGiovanni99 0:9bd4730782f9 88
PaoloGiovanni99 0:9bd4730782f9 89 }
PaoloGiovanni99 0:9bd4730782f9 90 }