Contest IOT GSE5 / Mbed 2 deprecated Nucleo_Robotik

En travaux...

Dependencies:   HCSR04 mbed

main.cpp@1:47b9204516a0, 2016-01-06 (annotated)

Committer:
Alex_Hochart
Date:
Wed Jan 06 15:20:09 2016 +0000
Revision:
1:47b9204516a0
Parent:
0:c085b14ea8df 
version fonctionnelle 2 capteurs

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Mickado 0:c085b14ea8df 1 #include "mbed.h"
Mickado 0:c085b14ea8df 2 #include "HCSR04.h"
Mickado 0:c085b14ea8df 3
Alex_Hochart 1:47b9204516a0 4 #define ECHO_FRONT D8
Alex_Hochart 1:47b9204516a0 5 #define TRIG_FRONT D9
Mickado 0:c085b14ea8df 6
Alex_Hochart 1:47b9204516a0 7 #define ECHO_BACK D10
Alex_Hochart 1:47b9204516a0 8 #define TRIG_BACK D11
Alex_Hochart 1:47b9204516a0 9
Mickado 0:c085b14ea8df 10
Mickado 0:c085b14ea8df 11
Alex_Hochart 1:47b9204516a0 12 Serial pc(USBTX, USBRX); //UART
Alex_Hochart 1:47b9204516a0 13 HCSR04 sensorFront(TRIG_FRONT, ECHO_FRONT);
Alex_Hochart 1:47b9204516a0 14 HCSR04 sensorBack(TRIG_BACK, ECHO_BACK);
Alex_Hochart 1:47b9204516a0 15
Alex_Hochart 1:47b9204516a0 16 PwmOut PWM_R(PB_3);
Alex_Hochart 1:47b9204516a0 17 PwmOut PWM_L(PB_10);
Alex_Hochart 1:47b9204516a0 18
Alex_Hochart 1:47b9204516a0 19 DigitalOut B_R(PB_5);
Alex_Hochart 1:47b9204516a0 20 DigitalOut F_R(PA_10);
Alex_Hochart 1:47b9204516a0 21
Alex_Hochart 1:47b9204516a0 22 DigitalOut B_L(PA_8);
Alex_Hochart 1:47b9204516a0 23 DigitalOut F_L(PB_4);
Mickado 0:c085b14ea8df 24
Alex_Hochart 1:47b9204516a0 25 /*
Alex_Hochart 1:47b9204516a0 26 * rotation infinitesimale vers la gauche, de puissance 0<P<1
Alex_Hochart 1:47b9204516a0 27 */
Alex_Hochart 1:47b9204516a0 28 void rotationG(float P){
Alex_Hochart 1:47b9204516a0 29 PWM_R = P;
Alex_Hochart 1:47b9204516a0 30 B_R = 0;
Alex_Hochart 1:47b9204516a0 31 F_R = 1;
Alex_Hochart 1:47b9204516a0 32 PWM_L = P;
Alex_Hochart 1:47b9204516a0 33 B_L = 0;
Alex_Hochart 1:47b9204516a0 34 F_L = 0;
Alex_Hochart 1:47b9204516a0 35 wait(0.05);
Alex_Hochart 1:47b9204516a0 36 }
Mickado 0:c085b14ea8df 37
Mickado 0:c085b14ea8df 38 /*
Alex_Hochart 1:47b9204516a0 39 * rotation infinitesimale vers la droite, de puissance 0<P<1
Mickado 0:c085b14ea8df 40 */
Alex_Hochart 1:47b9204516a0 41 void rotationD(float P){
Alex_Hochart 1:47b9204516a0 42 PWM_R = P;
Alex_Hochart 1:47b9204516a0 43 B_R = 0;
Alex_Hochart 1:47b9204516a0 44 F_R = 0;
Alex_Hochart 1:47b9204516a0 45 PWM_L = P;
Alex_Hochart 1:47b9204516a0 46 B_L = 0;
Alex_Hochart 1:47b9204516a0 47 F_L = 1;
Alex_Hochart 1:47b9204516a0 48 wait(0.05);
Alex_Hochart 1:47b9204516a0 49 }
Alex_Hochart 1:47b9204516a0 50
Alex_Hochart 1:47b9204516a0 51
Alex_Hochart 1:47b9204516a0 52 void virageGauche(int angle){}
Alex_Hochart 1:47b9204516a0 53
Alex_Hochart 1:47b9204516a0 54 //analyse des valeurs enregistrées --> angle de mur --> procédure de virage d'un angle donnée+ avancer 2cm --> mesure de distance --> correction angle --> avance
Alex_Hochart 1:47b9204516a0 55
Alex_Hochart 1:47b9204516a0 56 //Avancer tout droit
Alex_Hochart 1:47b9204516a0 57 void avancer(float P){
Alex_Hochart 1:47b9204516a0 58 PWM_R = P;
Alex_Hochart 1:47b9204516a0 59 B_R = 0;
Alex_Hochart 1:47b9204516a0 60 F_R = 1;
Alex_Hochart 1:47b9204516a0 61 PWM_L = P;
Alex_Hochart 1:47b9204516a0 62 B_L = 0;
Alex_Hochart 1:47b9204516a0 63 F_L = 1;
Alex_Hochart 1:47b9204516a0 64 wait(0.05);
Mickado 0:c085b14ea8df 65 }
Mickado 0:c085b14ea8df 66
Mickado 0:c085b14ea8df 67
Mickado 0:c085b14ea8df 68 int main() {
Alex_Hochart 1:47b9204516a0 69
Alex_Hochart 1:47b9204516a0 70 int distFront = 22;
Alex_Hochart 1:47b9204516a0 71 int distBack = 22;
Alex_Hochart 1:47b9204516a0 72 int difDist;
Alex_Hochart 1:47b9204516a0 73
Alex_Hochart 1:47b9204516a0 74 B_R = 0;
Alex_Hochart 1:47b9204516a0 75 F_R = 0;
Alex_Hochart 1:47b9204516a0 76 B_L = 0;
Alex_Hochart 1:47b9204516a0 77 F_L = 0;
Alex_Hochart 1:47b9204516a0 78
Alex_Hochart 1:47b9204516a0 79 PWM_R.period_ms(20);
Alex_Hochart 1:47b9204516a0 80 PWM_L.period_ms(20);
Alex_Hochart 1:47b9204516a0 81
Mickado 0:c085b14ea8df 82 while(1) {
Alex_Hochart 1:47b9204516a0 83
Alex_Hochart 1:47b9204516a0 84 distFront = sensorFront.distance(1);
Alex_Hochart 1:47b9204516a0 85 distBack = sensorBack.distance(1);
Alex_Hochart 1:47b9204516a0 86
Alex_Hochart 1:47b9204516a0 87 difDist = distBack - distFront;
Alex_Hochart 1:47b9204516a0 88
Alex_Hochart 1:47b9204516a0 89 if(difDist<-10){
Alex_Hochart 1:47b9204516a0 90 rotationG(0.8); //rotation à gauche
Alex_Hochart 1:47b9204516a0 91 }else if(difDist<0){
Alex_Hochart 1:47b9204516a0 92 rotationG(0.8); //rotation à gauche
Alex_Hochart 1:47b9204516a0 93 avancer(0.7);
Alex_Hochart 1:47b9204516a0 94 }else if(difDist>0){
Alex_Hochart 1:47b9204516a0 95 rotationD(0.8); //rotation à droite
Alex_Hochart 1:47b9204516a0 96 avancer(0.7);
Alex_Hochart 1:47b9204516a0 97 }else if(difDist>10){
Alex_Hochart 1:47b9204516a0 98 rotationG(0.8);
Alex_Hochart 1:47b9204516a0 99 }
Alex_Hochart 1:47b9204516a0 100
Mickado 0:c085b14ea8df 101 }
Mickado 0:c085b14ea8df 102 }
Alex_Hochart 1:47b9204516a0 103
Alex_Hochart 1:47b9204516a0 104 /* CODE POUR 1 CAPTEUR -------
Alex_Hochart 1:47b9204516a0 105 int main() {
Alex_Hochart 1:47b9204516a0 106
Alex_Hochart 1:47b9204516a0 107 int dist[3] = {0,0,0};
Alex_Hochart 1:47b9204516a0 108
Alex_Hochart 1:47b9204516a0 109 int distance = 0;
Alex_Hochart 1:47b9204516a0 110
Alex_Hochart 1:47b9204516a0 111 B_R = 0;
Alex_Hochart 1:47b9204516a0 112 F_R = 0;
Alex_Hochart 1:47b9204516a0 113 B_L = 0;
Alex_Hochart 1:47b9204516a0 114 F_L = 0;
Alex_Hochart 1:47b9204516a0 115
Alex_Hochart 1:47b9204516a0 116 PWM_R.period_ms(20);
Alex_Hochart 1:47b9204516a0 117 PWM_L.period_ms(20);
Alex_Hochart 1:47b9204516a0 118
Alex_Hochart 1:47b9204516a0 119 while(1) {
Alex_Hochart 1:47b9204516a0 120
Alex_Hochart 1:47b9204516a0 121 dist[2] = dist[1];
Alex_Hochart 1:47b9204516a0 122 dist[1] = dist[0];
Alex_Hochart 1:47b9204516a0 123 dist[0] = sensor.distance(1);
Alex_Hochart 1:47b9204516a0 124
Alex_Hochart 1:47b9204516a0 125 if(dist[0] > 40) dist[0] = 40;
Alex_Hochart 1:47b9204516a0 126 //distance = (dist[0] + dist[1] + dist[2])/3.0;
Alex_Hochart 1:47b9204516a0 127 //pc.printf("%d\n",dist);
Alex_Hochart 1:47b9204516a0 128 distance = dist[0];
Alex_Hochart 1:47b9204516a0 129
Alex_Hochart 1:47b9204516a0 130 if( distance < 20 ){
Alex_Hochart 1:47b9204516a0 131 rotationD(0.5); //rotation à droite
Alex_Hochart 1:47b9204516a0 132 }else if(distance > 30 ){
Alex_Hochart 1:47b9204516a0 133 rotationG(0.7); //rotation à gauche
Alex_Hochart 1:47b9204516a0 134 wait(0.05);
Alex_Hochart 1:47b9204516a0 135 avancer(0.6);
Alex_Hochart 1:47b9204516a0 136 }else if(distance > 25 ){
Alex_Hochart 1:47b9204516a0 137 rotationG(0.5); //rotation à gauche
Alex_Hochart 1:47b9204516a0 138 }else{
Alex_Hochart 1:47b9204516a0 139 avancer(0.7); //rotation à gauche
Alex_Hochart 1:47b9204516a0 140 }
Alex_Hochart 1:47b9204516a0 141 wait(0.05); //latence
Alex_Hochart 1:47b9204516a0 142 }
Alex_Hochart 1:47b9204516a0 143 }
Alex_Hochart 1:47b9204516a0 144 */