Xavier Jannin / Mbed 2 deprecated PETIT_robot

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

main.cpp@0:1cfd66c3a181, 2019-05-22 (annotated)

Committer:
xav_jann1
Date:
Wed May 22 16:54:27 2019 +0000
Revision:
0:1cfd66c3a181
Premiere version

Who changed what in which revision?

UserRevisionLine numberNew contents of line
xav_jann1 0:1cfd66c3a181 1 #include "mbed.h"
xav_jann1 0:1cfd66c3a181 2
xav_jann1 0:1cfd66c3a181 3 // Eléments:
xav_jann1 0:1cfd66c3a181 4 #include "Base.h"
xav_jann1 0:1cfd66c3a181 5 #include "Moteur.h"
xav_jann1 0:1cfd66c3a181 6 #include "Encodeur.h"
xav_jann1 0:1cfd66c3a181 7 #include "Odometrie.h"
xav_jann1 0:1cfd66c3a181 8 #include "Servo.h"
xav_jann1 0:1cfd66c3a181 9 #include "Infra.h"
xav_jann1 0:1cfd66c3a181 10 #include <math.h>
xav_jann1 0:1cfd66c3a181 11
xav_jann1 0:1cfd66c3a181 12 /*** Paramètres ***/
xav_jann1 0:1cfd66c3a181 13
xav_jann1 0:1cfd66c3a181 14 // Pin tirette:
xav_jann1 0:1cfd66c3a181 15 #define pin_tirette A4
xav_jann1 0:1cfd66c3a181 16
xav_jann1 0:1cfd66c3a181 17 // Pin interrupteur:
xav_jann1 0:1cfd66c3a181 18 #define pin_switch_color A5
xav_jann1 0:1cfd66c3a181 19
xav_jann1 0:1cfd66c3a181 20 // Pin Servo:
xav_jann1 0:1cfd66c3a181 21 #define pin_servo A0
xav_jann1 0:1cfd66c3a181 22
xav_jann1 0:1cfd66c3a181 23 // Pins des moteurs:
xav_jann1 0:1cfd66c3a181 24 #define pins_moteurG_enA_in1_in2 D8, D11, D12
xav_jann1 0:1cfd66c3a181 25 #define pins_moteurD_enB_in1_in2 D7, D14, D15
xav_jann1 0:1cfd66c3a181 26
xav_jann1 0:1cfd66c3a181 27 // Pin pompe à air:
xav_jann1 0:1cfd66c3a181 28 #define pin_pompe D6
xav_jann1 0:1cfd66c3a181 29
xav_jann1 0:1cfd66c3a181 30 // Pins Infrarouge:
xav_jann1 0:1cfd66c3a181 31 #define pin_infra1 A1
xav_jann1 0:1cfd66c3a181 32 #define pin_infra2 A2
xav_jann1 0:1cfd66c3a181 33 #define pin_infra3 A3
xav_jann1 0:1cfd66c3a181 34
xav_jann1 0:1cfd66c3a181 35 // Paramètres des encodeurs:
xav_jann1 0:1cfd66c3a181 36 #define encodeur_res 1024
xav_jann1 0:1cfd66c3a181 37 #define encodeur_diametre 40 // en mm
xav_jann1 0:1cfd66c3a181 38 #define encodeurs_entraxe 97/2.0f // en mm
xav_jann1 0:1cfd66c3a181 39
xav_jann1 0:1cfd66c3a181 40 // Paramètre de l'odometrie et asservissement:
xav_jann1 0:1cfd66c3a181 41 #define update_delay_us 500 // en µs
xav_jann1 0:1cfd66c3a181 42
xav_jann1 0:1cfd66c3a181 43 /*** Création des objets ***/
xav_jann1 0:1cfd66c3a181 44
xav_jann1 0:1cfd66c3a181 45 // Moteurs:
xav_jann1 0:1cfd66c3a181 46 Moteur moteurG(pins_moteurG_enA_in1_in2);
xav_jann1 0:1cfd66c3a181 47 Moteur moteurD(pins_moteurD_enB_in1_in2);
xav_jann1 0:1cfd66c3a181 48
xav_jann1 0:1cfd66c3a181 49 // Encodeurs:
xav_jann1 0:1cfd66c3a181 50 Encodeur encodeurG('G', encodeur_res, encodeur_diametre);
xav_jann1 0:1cfd66c3a181 51 Encodeur encodeurD('D', encodeur_res, encodeur_diametre);
xav_jann1 0:1cfd66c3a181 52
xav_jann1 0:1cfd66c3a181 53 // Odometrie:
xav_jann1 0:1cfd66c3a181 54 Odometrie odometrie(&encodeurG, &encodeurD, encodeurs_entraxe);
xav_jann1 0:1cfd66c3a181 55
xav_jann1 0:1cfd66c3a181 56 // Base:
xav_jann1 0:1cfd66c3a181 57 Base base(&moteurG, &moteurD, &odometrie, update_delay_us);
xav_jann1 0:1cfd66c3a181 58 #define base_max_erreur_distance 10
xav_jann1 0:1cfd66c3a181 59 #define base_max_erreur_orientation 0.1f
xav_jann1 0:1cfd66c3a181 60
xav_jann1 0:1cfd66c3a181 61 // Tirette:
xav_jann1 0:1cfd66c3a181 62 DigitalIn tirette(pin_tirette);
xav_jann1 0:1cfd66c3a181 63
xav_jann1 0:1cfd66c3a181 64 // Interrupteur:
xav_jann1 0:1cfd66c3a181 65 DigitalIn switch_color(pin_switch_color);
xav_jann1 0:1cfd66c3a181 66
xav_jann1 0:1cfd66c3a181 67 // Pompe:
xav_jann1 0:1cfd66c3a181 68 DigitalOut pompe(pin_pompe);
xav_jann1 0:1cfd66c3a181 69
xav_jann1 0:1cfd66c3a181 70 // Servo:
xav_jann1 0:1cfd66c3a181 71 Servo servo(pin_servo);
xav_jann1 0:1cfd66c3a181 72
xav_jann1 0:1cfd66c3a181 73 // Infrarouge:
xav_jann1 0:1cfd66c3a181 74 Infra infraFrontD(pin_infra1);
xav_jann1 0:1cfd66c3a181 75 Infra infraFrontG(pin_infra2);
xav_jann1 0:1cfd66c3a181 76 Infra infraBack(pin_infra3);
xav_jann1 0:1cfd66c3a181 77 int detection();
xav_jann1 0:1cfd66c3a181 78 bool isStopped = false;
xav_jann1 0:1cfd66c3a181 79
xav_jann1 0:1cfd66c3a181 80 // Buzzer:
xav_jann1 0:1cfd66c3a181 81 void buzzer_on();
xav_jann1 0:1cfd66c3a181 82 void buzzer_off();
xav_jann1 0:1cfd66c3a181 83 void buzzer_note();
xav_jann1 0:1cfd66c3a181 84
xav_jann1 0:1cfd66c3a181 85 // Fin de l'épreuve:
xav_jann1 0:1cfd66c3a181 86 #define epreuve_duration 100 // s
xav_jann1 0:1cfd66c3a181 87 Timeout epreuve_timeout;
xav_jann1 0:1cfd66c3a181 88 void finish() { base.stop(); }
xav_jann1 0:1cfd66c3a181 89
xav_jann1 0:1cfd66c3a181 90 // Prototypes - Machines d'états:
xav_jann1 0:1cfd66c3a181 91 extern int PURPLE_steps(int state);
xav_jann1 0:1cfd66c3a181 92 extern int YELLOW_steps(int state);
xav_jann1 0:1cfd66c3a181 93
xav_jann1 0:1cfd66c3a181 94 int main() {
xav_jann1 0:1cfd66c3a181 95
xav_jann1 0:1cfd66c3a181 96 printf("STM32 Base\r\n");
xav_jann1 0:1cfd66c3a181 97 printf("Adresses de variables: (pour STM-Studio)\r\n");
xav_jann1 0:1cfd66c3a181 98 printf(" - encodeurG.tours = %p\r\n", encodeurG.getTours_ptr());
xav_jann1 0:1cfd66c3a181 99 printf(" - encodeurD.tours = %p\r\n", encodeurD.getTours_ptr());
xav_jann1 0:1cfd66c3a181 100 printf(" - odometrie.X = %p\r\n", odometrie.getX_ptr());
xav_jann1 0:1cfd66c3a181 101 printf(" - odometrie.Y = %p\r\n", odometrie.getY_ptr());
xav_jann1 0:1cfd66c3a181 102 printf(" - odometrie.Theta = %p\r\n", odometrie.getTheta_ptr());
xav_jann1 0:1cfd66c3a181 103 printf(" - base.erreur_distance = %p\r\n", base.getErreur_distance_ptr());
xav_jann1 0:1cfd66c3a181 104 printf(" - base.erreur_orientation = %p\r\n", base.getErreur_orientation_ptr());
xav_jann1 0:1cfd66c3a181 105 printf(" - base.pwm_distance = %p\r\n", base.getPID_PWM_distance_ptr());
xav_jann1 0:1cfd66c3a181 106 printf(" - base.pwm_orientation = %p\r\n", base.getPID_PWM_orientation_ptr());
xav_jann1 0:1cfd66c3a181 107 printf(" - base.pwm_gauche = %p\r\n", base.getPID_PWM_gauche_ptr());
xav_jann1 0:1cfd66c3a181 108 printf(" - base.pwm_droite = %p\r\n", base.getPID_PWM_droite_ptr());
xav_jann1 0:1cfd66c3a181 109
xav_jann1 0:1cfd66c3a181 110 // PID:
xav_jann1 0:1cfd66c3a181 111 base.setPID_distance(1, 0, 1);
xav_jann1 0:1cfd66c3a181 112 base.setPID_orientation(10, 0, 1);
xav_jann1 0:1cfd66c3a181 113
xav_jann1 0:1cfd66c3a181 114 base.start();
xav_jann1 0:1cfd66c3a181 115
xav_jann1 0:1cfd66c3a181 116 // Devant:
xav_jann1 0:1cfd66c3a181 117 //base.forward(-1400);
xav_jann1 0:1cfd66c3a181 118 base.forward(1000);
xav_jann1 0:1cfd66c3a181 119 while(1) wait(1);
xav_jann1 0:1cfd66c3a181 120 //base.turn(360.0f*10);
xav_jann1 0:1cfd66c3a181 121 wait(6);
xav_jann1 0:1cfd66c3a181 122 base.turn(90.0f);
xav_jann1 0:1cfd66c3a181 123 wait(3);
xav_jann1 0:1cfd66c3a181 124 base.forward(-600);
xav_jann1 0:1cfd66c3a181 125 wait(3);
xav_jann1 0:1cfd66c3a181 126
xav_jann1 0:1cfd66c3a181 127 while(1) wait(1);
xav_jann1 0:1cfd66c3a181 128
xav_jann1 0:1cfd66c3a181 129 /*while(1) {
xav_jann1 0:1cfd66c3a181 130 buzzer_on();
xav_jann1 0:1cfd66c3a181 131 wait(4);
xav_jann1 0:1cfd66c3a181 132 buzzer_off();
xav_jann1 0:1cfd66c3a181 133 wait(2);
xav_jann1 0:1cfd66c3a181 134 }*/
xav_jann1 0:1cfd66c3a181 135
xav_jann1 0:1cfd66c3a181 136 /*while(1) {
xav_jann1 0:1cfd66c3a181 137 infraFrontG.mesure();
xav_jann1 0:1cfd66c3a181 138 infraFrontD.mesure();
xav_jann1 0:1cfd66c3a181 139 infraBack.mesure();
xav_jann1 0:1cfd66c3a181 140 float distanceG = infraFrontG.getDistance();
xav_jann1 0:1cfd66c3a181 141 float distanceD = infraFrontD.getDistance();
xav_jann1 0:1cfd66c3a181 142 float distanceB = infraBack.getDistance();
xav_jann1 0:1cfd66c3a181 143
xav_jann1 0:1cfd66c3a181 144 printf("f_G: %f \t f_D: %f \t f_B: %f\r\n", distanceG, distanceD, distanceB);
xav_jann1 0:1cfd66c3a181 145 wait(0.1);
xav_jann1 0:1cfd66c3a181 146 }*/
xav_jann1 0:1cfd66c3a181 147
xav_jann1 0:1cfd66c3a181 148 // Démarre le servo:
xav_jann1 0:1cfd66c3a181 149 servo.enable(1500, 20000);
xav_jann1 0:1cfd66c3a181 150 servo.pos(90);
xav_jann1 0:1cfd66c3a181 151
xav_jann1 0:1cfd66c3a181 152 // Tirette:
xav_jann1 0:1cfd66c3a181 153 char count = 0;
xav_jann1 0:1cfd66c3a181 154 /*while (count < 5) {
xav_jann1 0:1cfd66c3a181 155 if(!tirette) count++;
xav_jann1 0:1cfd66c3a181 156 else count = 0;
xav_jann1 0:1cfd66c3a181 157 wait(0.1);
xav_jann1 0:1cfd66c3a181 158 }*/
xav_jann1 0:1cfd66c3a181 159
xav_jann1 0:1cfd66c3a181 160 //wait(1);
xav_jann1 0:1cfd66c3a181 161
xav_jann1 0:1cfd66c3a181 162 // Fin de l'épreuve:
xav_jann1 0:1cfd66c3a181 163 epreuve_timeout.attach(&finish, epreuve_duration);
xav_jann1 0:1cfd66c3a181 164
xav_jann1 0:1cfd66c3a181 165 // Couleur du robot:
xav_jann1 0:1cfd66c3a181 166 bool color_side = 1;//switch_color;
xav_jann1 0:1cfd66c3a181 167
xav_jann1 0:1cfd66c3a181 168 // Démarre les moteurs:
xav_jann1 0:1cfd66c3a181 169 base.start();
xav_jann1 0:1cfd66c3a181 170
xav_jann1 0:1cfd66c3a181 171 // Eteint la pompe:
xav_jann1 0:1cfd66c3a181 172 pompe = 0;
xav_jann1 0:1cfd66c3a181 173
xav_jann1 0:1cfd66c3a181 174 // Désactive capteur arrière:
xav_jann1 0:1cfd66c3a181 175 infraBack.off();
xav_jann1 0:1cfd66c3a181 176
xav_jann1 0:1cfd66c3a181 177 // Déplacement:
xav_jann1 0:1cfd66c3a181 178 int time = 0, timeOut = 500, state = 0;
xav_jann1 0:1cfd66c3a181 179 while (1) {
xav_jann1 0:1cfd66c3a181 180 // Gestion du temps:
xav_jann1 0:1cfd66c3a181 181 wait_ms(1);
xav_jann1 0:1cfd66c3a181 182
xav_jann1 0:1cfd66c3a181 183 // Détection:
xav_jann1 0:1cfd66c3a181 184 //if (time % 50 == 0 || isStopped)
xav_jann1 0:1cfd66c3a181 185 // time += detection();
xav_jann1 0:1cfd66c3a181 186 //else
xav_jann1 0:1cfd66c3a181 187 time++;
xav_jann1 0:1cfd66c3a181 188
xav_jann1 0:1cfd66c3a181 189 // Etape suivante:
xav_jann1 0:1cfd66c3a181 190
xav_jann1 0:1cfd66c3a181 191 // Récupère les erreurs:
xav_jann1 0:1cfd66c3a181 192 //float erreur_distance = base.getErreurDistance();
xav_jann1 0:1cfd66c3a181 193 //float erreur_orientation = base.getErreurOrientation();
xav_jann1 0:1cfd66c3a181 194 //if (time % 100 == 0) printf("d: %f, o: %f\r\n", erreur_distance, erreur_orientation);
xav_jann1 0:1cfd66c3a181 195
xav_jann1 0:1cfd66c3a181 196 // Si la position est respectée:
xav_jann1 0:1cfd66c3a181 197 //if (state <= 11 && (time > timeOut || abs(erreur_distance) < base_max_erreur_distance && abs(erreur_orientation) < base_max_erreur_orientation)){
xav_jann1 0:1cfd66c3a181 198 if (state <= 14 && time > timeOut){
xav_jann1 0:1cfd66c3a181 199 if (color_side == 0) timeOut = PURPLE_steps(state);
xav_jann1 0:1cfd66c3a181 200 else timeOut = YELLOW_steps(state);
xav_jann1 0:1cfd66c3a181 201
xav_jann1 0:1cfd66c3a181 202 state++;
xav_jann1 0:1cfd66c3a181 203 time = 0;
xav_jann1 0:1cfd66c3a181 204 }
xav_jann1 0:1cfd66c3a181 205
xav_jann1 0:1cfd66c3a181 206 }
xav_jann1 0:1cfd66c3a181 207
xav_jann1 0:1cfd66c3a181 208 }
xav_jann1 0:1cfd66c3a181 209
xav_jann1 0:1cfd66c3a181 210 // Détection de l'environnement;
xav_jann1 0:1cfd66c3a181 211 int detection() {
xav_jann1 0:1cfd66c3a181 212 bool detectG = infraFrontG.mesure();
xav_jann1 0:1cfd66c3a181 213 bool detectD = infraFrontD.mesure();
xav_jann1 0:1cfd66c3a181 214 bool detectB = infraBack.mesure();
xav_jann1 0:1cfd66c3a181 215
xav_jann1 0:1cfd66c3a181 216 //if (detectG || detectD || detectB) {
xav_jann1 0:1cfd66c3a181 217 if (detectG || detectD) {
xav_jann1 0:1cfd66c3a181 218 base.stop();
xav_jann1 0:1cfd66c3a181 219 buzzer_on();
xav_jann1 0:1cfd66c3a181 220
xav_jann1 0:1cfd66c3a181 221 // Si première détection:
xav_jann1 0:1cfd66c3a181 222 if (isStopped == false) {
xav_jann1 0:1cfd66c3a181 223 isStopped = true;
xav_jann1 0:1cfd66c3a181 224 moteurG.setPWM(0.1);
xav_jann1 0:1cfd66c3a181 225 moteurD.setPWM(0.1);
xav_jann1 0:1cfd66c3a181 226 moteurG.forward();
xav_jann1 0:1cfd66c3a181 227 moteurD.forward();
xav_jann1 0:1cfd66c3a181 228 wait(0.5);
xav_jann1 0:1cfd66c3a181 229 moteurD.stop();
xav_jann1 0:1cfd66c3a181 230 moteurG.stop();
xav_jann1 0:1cfd66c3a181 231 } else {
xav_jann1 0:1cfd66c3a181 232 wait(1);
xav_jann1 0:1cfd66c3a181 233 }
xav_jann1 0:1cfd66c3a181 234
xav_jann1 0:1cfd66c3a181 235 } else if(detectB) {
xav_jann1 0:1cfd66c3a181 236 base.stop();
xav_jann1 0:1cfd66c3a181 237 buzzer_on();
xav_jann1 0:1cfd66c3a181 238
xav_jann1 0:1cfd66c3a181 239 // Si première détection:
xav_jann1 0:1cfd66c3a181 240 if (isStopped == false) {
xav_jann1 0:1cfd66c3a181 241 isStopped = true;
xav_jann1 0:1cfd66c3a181 242 moteurG.setPWM(0.1);
xav_jann1 0:1cfd66c3a181 243 moteurD.setPWM(0.1);
xav_jann1 0:1cfd66c3a181 244 moteurG.backward();
xav_jann1 0:1cfd66c3a181 245 moteurD.backward();
xav_jann1 0:1cfd66c3a181 246 wait(0.5);
xav_jann1 0:1cfd66c3a181 247 moteurD.stop();
xav_jann1 0:1cfd66c3a181 248 moteurG.stop();
xav_jann1 0:1cfd66c3a181 249 } else {
xav_jann1 0:1cfd66c3a181 250 wait(1);
xav_jann1 0:1cfd66c3a181 251 }
xav_jann1 0:1cfd66c3a181 252 }
xav_jann1 0:1cfd66c3a181 253 else {
xav_jann1 0:1cfd66c3a181 254 if (isStopped == true) base.start();
xav_jann1 0:1cfd66c3a181 255 buzzer_off();
xav_jann1 0:1cfd66c3a181 256 isStopped = false;
xav_jann1 0:1cfd66c3a181 257 return 1;
xav_jann1 0:1cfd66c3a181 258 }
xav_jann1 0:1cfd66c3a181 259
xav_jann1 0:1cfd66c3a181 260 return 0;
xav_jann1 0:1cfd66c3a181 261 }
xav_jann1 0:1cfd66c3a181 262
xav_jann1 0:1cfd66c3a181 263
xav_jann1 0:1cfd66c3a181 264 /*PwmOut buzzer(PB_1);
xav_jann1 0:1cfd66c3a181 265 Ticker buzzer_ticker;
xav_jann1 0:1cfd66c3a181 266 Timeout buzzer_timeout;
xav_jann1 0:1cfd66c3a181 267 DigitalOut buzzer_led(D2, 0);
xav_jann1 0:1cfd66c3a181 268 bool buzzer_state = false;
xav_jann1 0:1cfd66c3a181 269 */
xav_jann1 0:1cfd66c3a181 270 void buzzer_note() {
xav_jann1 0:1cfd66c3a181 271 /*float period;
xav_jann1 0:1cfd66c3a181 272 if (buzzer_state) period = 1000000.0f / 440.0f;
xav_jann1 0:1cfd66c3a181 273 else period = 1000000.0f / 349.0f;
xav_jann1 0:1cfd66c3a181 274 buzzer_state = !buzzer_state;
xav_jann1 0:1cfd66c3a181 275
xav_jann1 0:1cfd66c3a181 276 buzzer_led = !buzzer_led;
xav_jann1 0:1cfd66c3a181 277
xav_jann1 0:1cfd66c3a181 278 buzzer.period_us(period);
xav_jann1 0:1cfd66c3a181 279 buzzer.write(0.50f); // 50% duty cycle
xav_jann1 0:1cfd66c3a181 280 */
xav_jann1 0:1cfd66c3a181 281 }
xav_jann1 0:1cfd66c3a181 282
xav_jann1 0:1cfd66c3a181 283 void buzzer_turn_off() {
xav_jann1 0:1cfd66c3a181 284 /*buzzer_ticker.detach();
xav_jann1 0:1cfd66c3a181 285 buzzer.write(0);
xav_jann1 0:1cfd66c3a181 286 buzzer_led = 0;*/
xav_jann1 0:1cfd66c3a181 287 }
xav_jann1 0:1cfd66c3a181 288
xav_jann1 0:1cfd66c3a181 289 void buzzer_on() {
xav_jann1 0:1cfd66c3a181 290 //buzzer_ticker.attach(&buzzer_note, 0.4);
xav_jann1 0:1cfd66c3a181 291 }
xav_jann1 0:1cfd66c3a181 292
xav_jann1 0:1cfd66c3a181 293 void buzzer_off() {
xav_jann1 0:1cfd66c3a181 294 //buzzer_timeout.attach(&buzzer_turn_off, 1.0f);
xav_jann1 0:1cfd66c3a181 295 }