Contest IOT GSE5 / Mbed 2 deprecated Nucleo_Robotik_2

Good

Dependencies:   HCSR04 mbed

Fork of Nucleo_Robotik by Contest IOT GSE5

main.cpp@2:9b4d9afa44a3, 2016-01-06 (annotated)

Committer:
Alex_Hochart
Date:
Wed Jan 06 19:47:15 2016 +0000
Revision:
2:9b4d9afa44a3
Parent:
1:47b9204516a0 
Good;

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 2:9b4d9afa44a3 72 int difDist, distMoy;
Alex_Hochart 2:9b4d9afa44a3 73
Alex_Hochart 1:47b9204516a0 74
Alex_Hochart 1:47b9204516a0 75 B_R = 0;
Alex_Hochart 1:47b9204516a0 76 F_R = 0;
Alex_Hochart 1:47b9204516a0 77 B_L = 0;
Alex_Hochart 1:47b9204516a0 78 F_L = 0;
Alex_Hochart 1:47b9204516a0 79
Alex_Hochart 1:47b9204516a0 80 PWM_R.period_ms(20);
Alex_Hochart 1:47b9204516a0 81 PWM_L.period_ms(20);
Alex_Hochart 1:47b9204516a0 82
Mickado 0:c085b14ea8df 83 while(1) {
Alex_Hochart 1:47b9204516a0 84
Alex_Hochart 1:47b9204516a0 85 distFront = sensorFront.distance(1);
Alex_Hochart 1:47b9204516a0 86 distBack = sensorBack.distance(1);
Alex_Hochart 1:47b9204516a0 87
Alex_Hochart 1:47b9204516a0 88 difDist = distBack - distFront;
Alex_Hochart 2:9b4d9afa44a3 89 distMoy= (distBack + distFront)/2;
Alex_Hochart 1:47b9204516a0 90
Alex_Hochart 2:9b4d9afa44a3 91 if(distMoy<10){
Alex_Hochart 2:9b4d9afa44a3 92 //tourne à droite et avance
Alex_Hochart 2:9b4d9afa44a3 93 rotationD(0.6);
Alex_Hochart 2:9b4d9afa44a3 94 }else if(distMoy>30){
Alex_Hochart 2:9b4d9afa44a3 95 rotationG(0.6);
Alex_Hochart 2:9b4d9afa44a3 96 }
Alex_Hochart 2:9b4d9afa44a3 97 //notre programme
Alex_Hochart 2:9b4d9afa44a3 98
Alex_Hochart 1:47b9204516a0 99 if(difDist<-10){
Alex_Hochart 2:9b4d9afa44a3 100 rotationG(0.6); //rotation à gauche
Alex_Hochart 2:9b4d9afa44a3 101 }else if(difDist<-2){
Alex_Hochart 1:47b9204516a0 102 rotationG(0.8); //rotation à gauche
Alex_Hochart 1:47b9204516a0 103 avancer(0.7);
Alex_Hochart 2:9b4d9afa44a3 104 }else if(difDist>2){
Alex_Hochart 1:47b9204516a0 105 rotationD(0.8); //rotation à droite
Alex_Hochart 1:47b9204516a0 106 avancer(0.7);
Alex_Hochart 1:47b9204516a0 107 }else if(difDist>10){
Alex_Hochart 2:9b4d9afa44a3 108 rotationG(0.6);
Alex_Hochart 2:9b4d9afa44a3 109 }else {
Alex_Hochart 2:9b4d9afa44a3 110 avancer(0.7);
Alex_Hochart 1:47b9204516a0 111 }
Alex_Hochart 1:47b9204516a0 112
Mickado 0:c085b14ea8df 113 }
Mickado 0:c085b14ea8df 114 }
Alex_Hochart 1:47b9204516a0 115
Alex_Hochart 1:47b9204516a0 116 /* CODE POUR 1 CAPTEUR -------
Alex_Hochart 1:47b9204516a0 117 int main() {
Alex_Hochart 1:47b9204516a0 118
Alex_Hochart 1:47b9204516a0 119 int dist[3] = {0,0,0};
Alex_Hochart 1:47b9204516a0 120
Alex_Hochart 1:47b9204516a0 121 int distance = 0;
Alex_Hochart 1:47b9204516a0 122
Alex_Hochart 1:47b9204516a0 123 B_R = 0;
Alex_Hochart 1:47b9204516a0 124 F_R = 0;
Alex_Hochart 1:47b9204516a0 125 B_L = 0;
Alex_Hochart 1:47b9204516a0 126 F_L = 0;
Alex_Hochart 1:47b9204516a0 127
Alex_Hochart 1:47b9204516a0 128 PWM_R.period_ms(20);
Alex_Hochart 1:47b9204516a0 129 PWM_L.period_ms(20);
Alex_Hochart 1:47b9204516a0 130
Alex_Hochart 1:47b9204516a0 131 while(1) {
Alex_Hochart 1:47b9204516a0 132
Alex_Hochart 1:47b9204516a0 133 dist[2] = dist[1];
Alex_Hochart 1:47b9204516a0 134 dist[1] = dist[0];
Alex_Hochart 1:47b9204516a0 135 dist[0] = sensor.distance(1);
Alex_Hochart 1:47b9204516a0 136
Alex_Hochart 1:47b9204516a0 137 if(dist[0] > 40) dist[0] = 40;
Alex_Hochart 1:47b9204516a0 138 //distance = (dist[0] + dist[1] + dist[2])/3.0;
Alex_Hochart 1:47b9204516a0 139 //pc.printf("%d\n",dist);
Alex_Hochart 1:47b9204516a0 140 distance = dist[0];
Alex_Hochart 1:47b9204516a0 141
Alex_Hochart 1:47b9204516a0 142 if( distance < 20 ){
Alex_Hochart 1:47b9204516a0 143 rotationD(0.5); //rotation à droite
Alex_Hochart 1:47b9204516a0 144 }else if(distance > 30 ){
Alex_Hochart 1:47b9204516a0 145 rotationG(0.7); //rotation à gauche
Alex_Hochart 1:47b9204516a0 146 wait(0.05);
Alex_Hochart 1:47b9204516a0 147 avancer(0.6);
Alex_Hochart 1:47b9204516a0 148 }else if(distance > 25 ){
Alex_Hochart 1:47b9204516a0 149 rotationG(0.5); //rotation à gauche
Alex_Hochart 1:47b9204516a0 150 }else{
Alex_Hochart 1:47b9204516a0 151 avancer(0.7); //rotation à gauche
Alex_Hochart 1:47b9204516a0 152 }
Alex_Hochart 1:47b9204516a0 153 wait(0.05); //latence
Alex_Hochart 1:47b9204516a0 154 }
Alex_Hochart 1:47b9204516a0 155 }
Alex_Hochart 1:47b9204516a0 156 */