Yannick Andreu
/
Robot_Autonome_Proto
Andreu- Felix
main.cpp@0:afd478f066b0, 2017-07-03 (annotated)
- Committer:
- yannickrda
- Date:
- Mon Jul 03 10:13:36 2017 +0000
- Revision:
- 0:afd478f066b0
Final!!!
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
yannickrda | 0:afd478f066b0 | 1 | #include "mbed.h" |
yannickrda | 0:afd478f066b0 | 2 | #include "commande.h" |
yannickrda | 0:afd478f066b0 | 3 | #include <SRF08.h> |
yannickrda | 0:afd478f066b0 | 4 | #define adr 128 |
yannickrda | 0:afd478f066b0 | 5 | |
yannickrda | 0:afd478f066b0 | 6 | //*************************************************** |
yannickrda | 0:afd478f066b0 | 7 | //************** Déclaration des objets ************* |
yannickrda | 0:afd478f066b0 | 8 | //*************************************************** |
yannickrda | 0:afd478f066b0 | 9 | |
yannickrda | 0:afd478f066b0 | 10 | Serial bt(p13, p14); |
yannickrda | 0:afd478f066b0 | 11 | PwmOut pwm(p26); //objet |
yannickrda | 0:afd478f066b0 | 12 | Serial pc(USBTX,USBRX); |
yannickrda | 0:afd478f066b0 | 13 | SRF08 capt(p28,p27,0xE0); |
yannickrda | 0:afd478f066b0 | 14 | AnalogIn optique(p15); |
yannickrda | 0:afd478f066b0 | 15 | |
yannickrda | 0:afd478f066b0 | 16 | commande robot; |
yannickrda | 0:afd478f066b0 | 17 | |
yannickrda | 0:afd478f066b0 | 18 | //*************************************************** |
yannickrda | 0:afd478f066b0 | 19 | //************** Prototypes fonctions *************** |
yannickrda | 0:afd478f066b0 | 20 | //*************************************************** |
yannickrda | 0:afd478f066b0 | 21 | |
yannickrda | 0:afd478f066b0 | 22 | void PWM(void); // prototype fonction (cfr bas) |
yannickrda | 0:afd478f066b0 | 23 | void acqui(void); |
yannickrda | 0:afd478f066b0 | 24 | void autonomie(void); |
yannickrda | 0:afd478f066b0 | 25 | |
yannickrda | 0:afd478f066b0 | 26 | |
yannickrda | 0:afd478f066b0 | 27 | //*************************************************** |
yannickrda | 0:afd478f066b0 | 28 | //**************** Variables Gobales **************** |
yannickrda | 0:afd478f066b0 | 29 | //*************************************************** |
yannickrda | 0:afd478f066b0 | 30 | |
yannickrda | 0:afd478f066b0 | 31 | float pulse; |
yannickrda | 0:afd478f066b0 | 32 | int distance; |
yannickrda | 0:afd478f066b0 | 33 | unsigned c; |
yannickrda | 0:afd478f066b0 | 34 | float droite, gauche,devant,capteur; |
yannickrda | 0:afd478f066b0 | 35 | int right,left,straight,vu; |
yannickrda | 0:afd478f066b0 | 36 | |
yannickrda | 0:afd478f066b0 | 37 | //*************************************************** |
yannickrda | 0:afd478f066b0 | 38 | //********************** MAIN *********************** |
yannickrda | 0:afd478f066b0 | 39 | //*************************************************** |
yannickrda | 0:afd478f066b0 | 40 | |
yannickrda | 0:afd478f066b0 | 41 | int main() |
yannickrda | 0:afd478f066b0 | 42 | { |
yannickrda | 0:afd478f066b0 | 43 | bt.baud(57600); |
yannickrda | 0:afd478f066b0 | 44 | while(1) |
yannickrda | 0:afd478f066b0 | 45 | { |
yannickrda | 0:afd478f066b0 | 46 | autonomie(); |
yannickrda | 0:afd478f066b0 | 47 | /*while(1){ |
yannickrda | 0:afd478f066b0 | 48 | char choix = bt.getc(); |
yannickrda | 0:afd478f066b0 | 49 | |
yannickrda | 0:afd478f066b0 | 50 | if(choix=='0'){ |
yannickrda | 0:afd478f066b0 | 51 | robot.avancer(50);bt.printf("avance\n");} |
yannickrda | 0:afd478f066b0 | 52 | else if(choix=='1'){ |
yannickrda | 0:afd478f066b0 | 53 | robot.reculer(50);bt.printf("recule\n");} |
yannickrda | 0:afd478f066b0 | 54 | else if(choix=='2'){ |
yannickrda | 0:afd478f066b0 | 55 | robot.tourner_gauche(60);bt.printf("gauche\n");} |
yannickrda | 0:afd478f066b0 | 56 | else if(choix=='3'){ |
yannickrda | 0:afd478f066b0 | 57 | robot.tourner_droite(60);bt.printf("droite\n");} |
yannickrda | 0:afd478f066b0 | 58 | else if(choix=='4'){ |
yannickrda | 0:afd478f066b0 | 59 | robot.stop();bt.printf("stop\n\r");} |
yannickrda | 0:afd478f066b0 | 60 | else if(choix=='5') |
yannickrda | 0:afd478f066b0 | 61 | break; |
yannickrda | 0:afd478f066b0 | 62 | }*/ |
yannickrda | 0:afd478f066b0 | 63 | } |
yannickrda | 0:afd478f066b0 | 64 | } |
yannickrda | 0:afd478f066b0 | 65 | |
yannickrda | 0:afd478f066b0 | 66 | //**************************************************** |
yannickrda | 0:afd478f066b0 | 67 | //******************** Fonctions ********************* |
yannickrda | 0:afd478f066b0 | 68 | //**************************************************** |
yannickrda | 0:afd478f066b0 | 69 | |
yannickrda | 0:afd478f066b0 | 70 | |
yannickrda | 0:afd478f066b0 | 71 | /*void direction ( int x) //objectif faire le CHOIX de la meilleure direction |
yannickrda | 0:afd478f066b0 | 72 | { |
yannickrda | 0:afd478f066b0 | 73 | if((straight < (left + 10)) && (straight < (right + 10)) |
yannickrda | 0:afd478f066b0 | 74 | {robot.avancer(50);bt.printf("avance\n");} |
yannickrda | 0:afd478f066b0 | 75 | if((left > (right + 15)) && (left >= (straight + 15))){ // pourquoi le + XX ? parce que le capteur est trop sensible pour varier suivant l'unité choisie |
yannickrda | 0:afd478f066b0 | 76 | robot.tourner_droite(60);bt.printf("droite\n"); // comparaison capteur relevé devant, à gauche et à droite, => la valeur la plus faible (= obstacle le + loin) l'emporte |
yannickrda | 0:afd478f066b0 | 77 | robot.avancer(50);bt.printf("avance\n");} |
yannickrda | 0:afd478f066b0 | 78 | if((right > (left + 15)) && (right >= (straight + 15))){ |
yannickrda | 0:afd478f066b0 | 79 | robot.tourner_gauche(60);bt.printf("gauche\n"); |
yannickrda | 0:afd478f066b0 | 80 | robot.avancer(50);bt.printf("avance\n");} |
yannickrda | 0:afd478f066b0 | 81 | }*/ |
yannickrda | 0:afd478f066b0 | 82 | void autonomie() |
yannickrda | 0:afd478f066b0 | 83 | { |
yannickrda | 0:afd478f066b0 | 84 | pwm.period_ms(20); //définition da la largeur de l'impulsion: 20 ms |
yannickrda | 0:afd478f066b0 | 85 | pulse=0.0715; |
yannickrda | 0:afd478f066b0 | 86 | |
yannickrda | 0:afd478f066b0 | 87 | while(1) |
yannickrda | 0:afd478f066b0 | 88 | { |
yannickrda | 0:afd478f066b0 | 89 | |
yannickrda | 0:afd478f066b0 | 90 | pulse=0.0715;pwm.write(pulse);acqui(); //capteur centre |
yannickrda | 0:afd478f066b0 | 91 | devant=vu; straight=capteur; |
yannickrda | 0:afd478f066b0 | 92 | if(straight>50) |
yannickrda | 0:afd478f066b0 | 93 | robot.avance(50); |
yannickrda | 0:afd478f066b0 | 94 | while(devant<60 && straight<50) |
yannickrda | 0:afd478f066b0 | 95 | { |
yannickrda | 0:afd478f066b0 | 96 | robot.stop(); |
yannickrda | 0:afd478f066b0 | 97 | pulse=0.095;wait(0.03);pwm.write(pulse);acqui(); //capteur gauche |
yannickrda | 0:afd478f066b0 | 98 | gauche=vu; left=capteur; |
yannickrda | 0:afd478f066b0 | 99 | |
yannickrda | 0:afd478f066b0 | 100 | pulse=0.048;wait(0.03);pwm.write(pulse);acqui(); //dcapteur roite |
yannickrda | 0:afd478f066b0 | 101 | droite=vu; right=capteur; |
yannickrda | 0:afd478f066b0 | 102 | |
yannickrda | 0:afd478f066b0 | 103 | if(gauche<droite) |
yannickrda | 0:afd478f066b0 | 104 | { |
yannickrda | 0:afd478f066b0 | 105 | robot.tourner_gauche(40); |
yannickrda | 0:afd478f066b0 | 106 | wait(0.6); |
yannickrda | 0:afd478f066b0 | 107 | robot.stop(); |
yannickrda | 0:afd478f066b0 | 108 | } |
yannickrda | 0:afd478f066b0 | 109 | else if(gauche>droite) |
yannickrda | 0:afd478f066b0 | 110 | { |
yannickrda | 0:afd478f066b0 | 111 | robot.tourner_droite(40); |
yannickrda | 0:afd478f066b0 | 112 | wait(0.6); |
yannickrda | 0:afd478f066b0 | 113 | robot.stop(); |
yannickrda | 0:afd478f066b0 | 114 | } |
yannickrda | 0:afd478f066b0 | 115 | pulse=0.0715;wait(0.03);pwm.write(pulse);acqui(); //capteur centre |
yannickrda | 0:afd478f066b0 | 116 | devant=vu; straight=capteur; |
yannickrda | 0:afd478f066b0 | 117 | } |
yannickrda | 0:afd478f066b0 | 118 | } |
yannickrda | 0:afd478f066b0 | 119 | } |
yannickrda | 0:afd478f066b0 | 120 | /* |
yannickrda | 0:afd478f066b0 | 121 | pulse=0.095;pwm.write(pulse);acqui(); //gauche |
yannickrda | 0:afd478f066b0 | 122 | gauche=vu; left=capteur; |
yannickrda | 0:afd478f066b0 | 123 | pulse=0.0715;pwm.write(pulse);acqui(); //centre |
yannickrda | 0:afd478f066b0 | 124 | devant=vu; straight=capteur; |
yannickrda | 0:afd478f066b0 | 125 | pulse=0.048;pwm.write(pulse);acqui(); //droite |
yannickrda | 0:afd478f066b0 | 126 | droite=vu; right=capteur;*/ |
yannickrda | 0:afd478f066b0 | 127 | |
yannickrda | 0:afd478f066b0 | 128 | void acqui() |
yannickrda | 0:afd478f066b0 | 129 | { |
yannickrda | 0:afd478f066b0 | 130 | vu = optique.read_u16(); |
yannickrda | 0:afd478f066b0 | 131 | vu = 422006/(vu-3475); |
yannickrda | 0:afd478f066b0 | 132 | capteur = capt.read(); |
yannickrda | 0:afd478f066b0 | 133 | bt.printf("Sonar : %f.2\n",capt.read()); |
yannickrda | 0:afd478f066b0 | 134 | bt.printf("Optique : %d\n",vu); |
yannickrda | 0:afd478f066b0 | 135 | wait(0.1); |
yannickrda | 0:afd478f066b0 | 136 | } |