sblas / IMA4P7

Commit cf4b546b54d99f18e800a25827492f0966ac5e47

Authored by sblas
1 parent d4a6dcca

Ajout du programme pour le guidage du robot

Showing 1 changed file with 202 additions and 0 deletions   Show diff stats
Inline Side-by-side
Programme_arduino_robot 0 → 100644
  Show/Hide comments   View file @cf4b546
... ... @@ -0,0 +1,202 @@
  1 +#include <SoftwareSerial.h>
  2 +
  3 +SoftwareSerial soft2(8,9);
  4 +
  5 +int pwma = 10;
  6 +int pwmb = 11;
  7 +int ain1 = 7;
  8 +int ain2 = 6;
  9 +int bin1 = 5;
  10 +int bin2 = 4;
  11 +int stby = 3;
  12 +int TRIG = A1;
  13 +int ECHO = A0;
  14 +long cm;
  15 +long lect_echo;
  16 + int envoi=0;
  17 +
  18 +void setup() {
  19 + Serial.begin(9600);
  20 + soft2.begin(38400);
  21 + pinMode(TRIG,OUTPUT);
  22 +digitalWrite(TRIG, LOW);
  23 +pinMode(ECHO,INPUT);
  24 +}
  25 +
  26 +void loop() {
  27 + String msg;
  28 + char inByte='a';
  29 + int i = 0;
  30 + int nb_if = 0;
  31 + char id_sauv;
  32 + int test_if;
  33 + int nb_boucles[6];
  34 + char id_boucles[6];
  35 + int boucle_rec = 0;
  36 + int capteur;
  37 + //boucle dans une boucle gérée
  38 +
  39 + /* soft2.listen();
  40 + while(soft2.available()>0 && inByte !='\n'){
  41 + inByte=soft2.read();
  42 + msg+=inByte;
  43 + delay(10);
  44 + i++;
  45 + envoi=1;
  46 + }*/
  47 + if(envoi==0){
  48 + Serial.println("etape 0");
  49 + delay(20000);
  50 + msg += (char)0x10;
  51 + msg += (char)0x61;
  52 + msg += (char)0x0F;
  53 + msg += (char)0x20;
  54 + msg += (char)0x10;
  55 + msg += (char)0x70;
  56 + msg += (char)0x10;
  57 + msg += (char)0x61;
  58 + msg += (char)0x0F;
  59 + msg += (char)0x40;
  60 + msg += (char)0x04;
  61 + msg += (char)0x30;
  62 + msg += (char)0x50;
  63 + msg += (char)0x70;
  64 + i = 14;
  65 + envoi=1;
  66 + Serial.println("etape 1");
  67 + }
  68 + //++a01z++a++z test if
  69 + //++B 1Q+z test while (executé 32 fois)
  70 + //++B 1B +Q1Q+z test while dans un while
  71 + if(envoi==1){
  72 + Serial.println("étape 2");
  73 + for(int k=0;k<i;k++){
  74 + if((msg.charAt(k) & 0xF0) ==0x10){
  75 + Serial.print("tout droit\n\r");
  76 + digitalWrite(stby,HIGH);
  77 + analogWrite(pwma,100);
  78 + analogWrite(pwmb,100);
  79 + digitalWrite(ain1,HIGH);
  80 + digitalWrite(bin1,HIGH);
  81 + digitalWrite(ain2,LOW);
  82 + digitalWrite(bin2,LOW);
  83 + delay(1000);
  84 + }
  85 +
  86 + else if((msg.charAt(k) & 0xF0) ==0x20){
  87 + Serial.print("à gauche\n\r");
  88 + digitalWrite(stby,HIGH);
  89 + analogWrite(pwma,100);
  90 + analogWrite(pwmb,100);
  91 + digitalWrite(ain1,HIGH);
  92 + digitalWrite(bin1,LOW);
  93 + digitalWrite(ain2,LOW);
  94 + digitalWrite(bin2,HIGH);
  95 + delay(900);
  96 + }
  97 +
  98 + else if((msg.charAt(k) & 0xF0) ==0x30){
  99 + Serial.print("à droite\n\r");
  100 + digitalWrite(stby,HIGH);
  101 + analogWrite(pwma,100);
  102 + analogWrite(pwmb,100);
  103 + digitalWrite(ain1,LOW);
  104 + digitalWrite(bin1,HIGH);
  105 + digitalWrite(ain2,HIGH);
  106 + digitalWrite(bin2,LOW);
  107 + delay(900);
  108 + }
  109 +
  110 + else if((msg.charAt(k) & 0xF0) ==0x40){
  111 + Serial.print("début while\n\r");
  112 + k++;
  113 + nb_boucles[boucle_rec] = (int)msg.charAt(k);
  114 + id_boucles[boucle_rec] = msg.charAt(k-1);
  115 + boucle_rec++;
  116 + }
  117 +
  118 + else if((msg.charAt(k) & 0xF0) ==0x50){
  119 + nb_boucles[boucle_rec-1]--;
  120 + if(nb_boucles[boucle_rec-1]!=0){
  121 + k--;
  122 + while(msg.charAt(k)!= id_boucles[boucle_rec-1]){
  123 + k--;
  124 + }
  125 + k++;
  126 + }
  127 + else{
  128 + boucle_rec--;
  129 + Serial.print("fin while\n\r");
  130 + }
  131 + }
  132 +
  133 + else if((msg.charAt(k) & 0xF0) ==0x60){
  134 + Serial.print("if\n\r");
  135 + digitalWrite(TRIG, HIGH);
  136 + delayMicroseconds(50);
  137 + digitalWrite(TRIG, LOW);
  138 + lect_echo = pulseIn(ECHO, HIGH);
  139 + cm = lect_echo / 58;
  140 + Serial.println(cm);
  141 + delay(10);
  142 + if((msg.charAt(k) & 0x01) == 0x01){
  143 + if(msg.charAt(++k) > cm){
  144 + nb_if++;
  145 + test_if=nb_if;
  146 + id_sauv = msg.charAt(k+1);
  147 + while(((msg.charAt(k)) & 0xF0) != 0x70 && k<i){
  148 + k++;
  149 + }
  150 + while(test_if!=0){
  151 + if(((msg.charAt(k) & 0xF0) == 0x60)){
  152 + k++;
  153 + test_if--;
  154 + }
  155 + else k++;
  156 + }
  157 + }
  158 + }
  159 + else if((msg.charAt(k) & 0x01) == 0x00){
  160 + if(msg.charAt(++k) < cm){
  161 + nb_if++;
  162 + test_if=nb_if;
  163 + id_sauv = msg.charAt(k+1);
  164 + while(((msg.charAt(k)) & 0xF0) != 0x70 && k<i){
  165 + k++;
  166 + }
  167 + while(test_if!=0){
  168 + if(((msg.charAt(k) & 0xF0) == 0x60)){
  169 + k++;
  170 + test_if--;
  171 + }
  172 + else k++;
  173 + }
  174 + }
  175 + }
  176 + }
  177 +
  178 + else if((msg.charAt(k) & 0xF0) ==0x70){
  179 + Serial.print("fin parcours\n\r");
  180 + envoi=2;
  181 + digitalWrite(stby,LOW);
  182 + analogWrite(pwma,0);
  183 + analogWrite(pwmb,0);
  184 + digitalWrite(ain1,LOW);
  185 + digitalWrite(bin1,LOW);
  186 + digitalWrite(ain2,LOW);
  187 + digitalWrite(bin2,LOW);
  188 + k=i;
  189 + }
  190 +
  191 + }
  192 + }
  193 +}
  194 +
  195 +
  196 +/*- Tout droit: 0001
  197 +- Virage à gauche: 0010
  198 +- Virage à droite: 0011
  199 +- Début while: 0100
  200 +- Fin while: 0101
  201 +- If: 0110
  202 +- Fin parcours: 0111*/
0 203 \ No newline at end of file
... ...