Davy MILLION / Mbed 2 deprecated Sumo

issou

Dependencies:   mbed hcsr04

main.cpp@1:32df7d7d355d, 2021-03-26 (annotated)

Committer:
Davdav
Date:
Fri Mar 26 15:50:42 2021 +0000
Revision:
1:32df7d7d355d
Parent:
0:6f23e23056ac 
a

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Davdav 0:6f23e23056ac 1 #include "mbed.h"
Davdav 1:32df7d7d355d 2 #include "hcsr04.h"
Davdav 0:6f23e23056ac 3
Davdav 1:32df7d7d355d 4 #define MOT_A_IN_1 D3
Davdav 1:32df7d7d355d 5 #define MOT_A_IN_2 D4
Davdav 1:32df7d7d355d 6 #define MOT_A_PWM D5
Davdav 0:6f23e23056ac 7
Davdav 1:32df7d7d355d 8 #define MOT_B_IN_1 D9
Davdav 1:32df7d7d355d 9 #define MOT_B_IN_2 D7
Davdav 1:32df7d7d355d 10 #define MOT_B_PWM D6
Davdav 1:32df7d7d355d 11
Davdav 1:32df7d7d355d 12 #define STBY D2
Davdav 0:6f23e23056ac 13
Davdav 1:32df7d7d355d 14 #define ECHO D11
Davdav 1:32df7d7d355d 15 #define TRIGGER D12
Davdav 0:6f23e23056ac 16
Davdav 1:32df7d7d355d 17 #define LINE_LEFT D14
Davdav 1:32df7d7d355d 18 #define LINE_RIGHT D15
Davdav 1:32df7d7d355d 19
Davdav 1:32df7d7d355d 20 #define EPSILON 50
Davdav 0:6f23e23056ac 21
Davdav 1:32df7d7d355d 22 PwmOut speedA(MOT_A_PWM);
Davdav 1:32df7d7d355d 23 PwmOut speedB(MOT_B_PWM);
Davdav 1:32df7d7d355d 24
Davdav 1:32df7d7d355d 25 //DigitalOut myled(LED1);
Davdav 1:32df7d7d355d 26 HCSR04 sonar(TRIGGER, ECHO);
Davdav 1:32df7d7d355d 27 Serial pc(USBTX, USBRX);
Davdav 1:32df7d7d355d 28
Davdav 0:6f23e23056ac 29 DigitalOut inA1(MOT_A_IN_1);
Davdav 0:6f23e23056ac 30 DigitalOut inA2(MOT_A_IN_2);
Davdav 0:6f23e23056ac 31 DigitalOut inB1(MOT_B_IN_1);
Davdav 0:6f23e23056ac 32 DigitalOut inB2(MOT_B_IN_2);
Davdav 1:32df7d7d355d 33
Davdav 0:6f23e23056ac 34 DigitalOut stby(STBY);
Davdav 1:32df7d7d355d 35
Davdav 1:32df7d7d355d 36 DigitalIn lineL(LINE_LEFT); // ?+
Davdav 0:6f23e23056ac 37 DigitalIn lineR(LINE_RIGHT); // ?
Davdav 1:32df7d7d355d 38
Davdav 1:32df7d7d355d 39 // ajout du
Davdav 0:6f23e23056ac 40
Davdav 0:6f23e23056ac 41 void testMotors();
Davdav 1:32df7d7d355d 42 void testLineSensorsCustom();
Davdav 1:32df7d7d355d 43 void DetectThing();
Davdav 0:6f23e23056ac 44
Davdav 0:6f23e23056ac 45 int main() {
Davdav 1:32df7d7d355d 46 //pc.baud(9600);
Davdav 1:32df7d7d355d 47 //pc.printf("Hello World");
Davdav 0:6f23e23056ac 48 //testMotors();
Davdav 1:32df7d7d355d 49 //testLineSensorsCustom();
Davdav 1:32df7d7d355d 50 //DetectThing();
Davdav 1:32df7d7d355d 51
Davdav 1:32df7d7d355d 52 // Choix de la direction des moteurs, le robot avance.
Davdav 1:32df7d7d355d 53 /*
Davdav 1:32df7d7d355d 54 inA1 = 0;
Davdav 1:32df7d7d355d 55 inA2 = 1;
Davdav 1:32df7d7d355d 56 inB1 = 1;
Davdav 1:32df7d7d355d 57 inB2 = 0;*/
Davdav 1:32df7d7d355d 58 //DetectThing();
Davdav 1:32df7d7d355d 59
Davdav 1:32df7d7d355d 60 // Choix de la vitesse de rotation des moteurs.
Davdav 1:32df7d7d355d 61 //speedA = 0.3; // Moteur gauche
Davdav 1:32df7d7d355d 62 //speedB = 0.3; // Moteur droit
Davdav 1:32df7d7d355d 63 // vpourcentage par rapport à la vitesse max
Davdav 1:32df7d7d355d 64 testMotors();
Davdav 1:32df7d7d355d 65 // On active les moteurs
Davdav 1:32df7d7d355d 66 //stby = 1;
Davdav 1:32df7d7d355d 67
Davdav 1:32df7d7d355d 68 }
Davdav 1:32df7d7d355d 69
Davdav 1:32df7d7d355d 70 void DetectThing()
Davdav 1:32df7d7d355d 71 {
Davdav 1:32df7d7d355d 72 double dist;
Davdav 1:32df7d7d355d 73
Davdav 1:32df7d7d355d 74 while (1)
Davdav 1:32df7d7d355d 75 {
Davdav 1:32df7d7d355d 76 sonar.start();
Davdav 1:32df7d7d355d 77 dist = sonar.get_dist_cm();
Davdav 1:32df7d7d355d 78
Davdav 1:32df7d7d355d 79 pc.printf("distance : %f\r\n", dist);
Davdav 1:32df7d7d355d 80 //wait(0.2);
Davdav 1:32df7d7d355d 81 }
Davdav 1:32df7d7d355d 82
Davdav 0:6f23e23056ac 83 }
Davdav 0:6f23e23056ac 84
Davdav 0:6f23e23056ac 85 void testMotors()
Davdav 0:6f23e23056ac 86 {
Davdav 0:6f23e23056ac 87 // Choix de la direction des moteurs, le robot avance.
Davdav 0:6f23e23056ac 88 inA1 = 1;
Davdav 0:6f23e23056ac 89 inA2 = 0;
Davdav 0:6f23e23056ac 90 inB1 = 1;
Davdav 0:6f23e23056ac 91 inB2 = 0;
Davdav 0:6f23e23056ac 92
Davdav 0:6f23e23056ac 93 // Choix de la vitesse de rotation des moteurs.
Davdav 1:32df7d7d355d 94 speedA = 0.01; // Moteur gauche
Davdav 1:32df7d7d355d 95 speedB = 0.01; // Moteur droit
Davdav 1:32df7d7d355d 96 // vpourcentage par rapport à la vitesse max
Davdav 0:6f23e23056ac 97
Davdav 0:6f23e23056ac 98 // On active les moteurs
Davdav 0:6f23e23056ac 99 stby = 1;
Davdav 1:32df7d7d355d 100 // si stby à 0 ? alors moteur en standby, enfaite stby est inv stby
Davdav 0:6f23e23056ac 101
Davdav 1:32df7d7d355d 102 wait(0.5); // On avance pendant 1 secondes
Davdav 0:6f23e23056ac 103
Davdav 0:6f23e23056ac 104 // On tourne pendant 1sec sur la gauche
Davdav 0:6f23e23056ac 105 speedA = 0.5;
Davdav 1:32df7d7d355d 106 wait(0.5);
Davdav 0:6f23e23056ac 107
Davdav 0:6f23e23056ac 108 // On tourne pendant 1sec sur la droite
Davdav 0:6f23e23056ac 109 speedA = 1;
Davdav 0:6f23e23056ac 110 speedB = 0.5;
Davdav 1:32df7d7d355d 111 wait(0.5);
Davdav 0:6f23e23056ac 112
Davdav 0:6f23e23056ac 113 // On tourne sur place sur la gauche pendant 1sec
Davdav 0:6f23e23056ac 114 speedB = 1;
Davdav 0:6f23e23056ac 115 inA1 = 0;
Davdav 0:6f23e23056ac 116 inA2 = 1;
Davdav 1:32df7d7d355d 117 wait(0.5);
Davdav 0:6f23e23056ac 118
Davdav 0:6f23e23056ac 119 // On tourne sur place sur la droite pendant 1sec
Davdav 0:6f23e23056ac 120 inA1 = 1;
Davdav 0:6f23e23056ac 121 inA2 = 0;
Davdav 0:6f23e23056ac 122 inB1 = 0;
Davdav 0:6f23e23056ac 123 inB2 = 1;
Davdav 1:32df7d7d355d 124 wait(0.5);
Davdav 0:6f23e23056ac 125
Davdav 0:6f23e23056ac 126 // On recule pendant 1sec;
Davdav 0:6f23e23056ac 127 inA1 = 0;
Davdav 0:6f23e23056ac 128 inA2 = 1;
Davdav 1:32df7d7d355d 129 wait(0.5);
Davdav 0:6f23e23056ac 130
Davdav 0:6f23e23056ac 131 // On avance de nouveau 1sec;
Davdav 0:6f23e23056ac 132 inA1 = 1;
Davdav 0:6f23e23056ac 133 inA2 = 0;
Davdav 0:6f23e23056ac 134 inB1 = 1;
Davdav 0:6f23e23056ac 135 inB2 = 0;
Davdav 1:32df7d7d355d 136 wait(0.5);
Davdav 0:6f23e23056ac 137
Davdav 0:6f23e23056ac 138 speedA=0;
Davdav 0:6f23e23056ac 139 speedB=0;
Davdav 0:6f23e23056ac 140 }
Davdav 1:32df7d7d355d 141 /*
Davdav 1:32df7d7d355d 142 void testLineSensorsCustom()
Davdav 0:6f23e23056ac 143 {
Davdav 1:32df7d7d355d 144 float dist;
Davdav 1:32df7d7d355d 145
Davdav 0:6f23e23056ac 146 inA1 = 1;
Davdav 0:6f23e23056ac 147 inA2 = 0;
Davdav 0:6f23e23056ac 148 inB1 = 1;
Davdav 0:6f23e23056ac 149 inB2 = 0;
Davdav 0:6f23e23056ac 150
Davdav 0:6f23e23056ac 151 stby = 1;
Davdav 0:6f23e23056ac 152
Davdav 0:6f23e23056ac 153 speedA = 0;
Davdav 0:6f23e23056ac 154 speedB = 0;
Davdav 0:6f23e23056ac 155
Davdav 1:32df7d7d355d 156 while(dist > EPSILON/2)
Davdav 1:32df7d7d355d 157 {
Davdav 1:32df7d7d355d 158 sonar.start();
Davdav 1:32df7d7d355d 159 dist = sonar.get_dist_cm();
Davdav 1:32df7d7d355d 160
Davdav 1:32df7d7d355d 161 //pc.printf("distance : %f\r\n", dist);
Davdav 1:32df7d7d355d 162
Davdav 1:32df7d7d355d 163 speedA = 1;
Davdav 1:32df7d7d355d 164 speedB = .9;
Davdav 1:32df7d7d355d 165 inA1 = 1;
Davdav 1:32df7d7d355d 166 inA2 = 0;
Davdav 1:32df7d7d355d 167 inB1 = 1;
Davdav 1:32df7d7d355d 168 inB2 = 0;
Davdav 1:32df7d7d355d 169 }
Davdav 1:32df7d7d355d 170
Davdav 1:32df7d7d355d 171
Davdav 1:32df7d7d355d 172 while(dist < EPSILON/2)
Davdav 0:6f23e23056ac 173 {
Davdav 1:32df7d7d355d 174 sonar.start();
Davdav 1:32df7d7d355d 175 dist = sonar.get_dist_cm();
Davdav 1:32df7d7d355d 176
Davdav 1:32df7d7d355d 177 speedA = 0;
Davdav 1:32df7d7d355d 178 speedB = 0;
Davdav 1:32df7d7d355d 179 inA1 = inA2 = inB1 = inB2 = 1;
Davdav 1:32df7d7d355d 180
Davdav 1:32df7d7d355d 181 }
Davdav 1:32df7d7d355d 182 }
Davdav 1:32df7d7d355d 183
Davdav 1:32df7d7d355d 184
Davdav 1:32df7d7d355d 185
Davdav 1:32df7d7d355d 186 -> Choix direction
Davdav 1:32df7d7d355d 187 -> Choix vitesse rotation moteur
Davdav 1:32df7d7d355d 188
Davdav 1:32df7d7d355d 189
Davdav 1:32df7d7d355d 190
Davdav 1:32df7d7d355d 191 if (lineL <= .5)
Davdav 1:32df7d7d355d 192 speedA = 0.9;
Davdav 0:6f23e23056ac 193 else
Davdav 0:6f23e23056ac 194 speedA = 0;
Davdav 0:6f23e23056ac 195 if (lineR <= .5) // ??
Davdav 0:6f23e23056ac 196 speedB = .7;
Davdav 0:6f23e23056ac 197 else
Davdav 0:6f23e23056ac 198 speedB = 0;
Davdav 1:32df7d7d355d 199 */