SHARE
TWEET

Untitled

RenabaReD Dec 5th, 2019 72 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #define INTERVAL 1000
  2. #define P_TURN 10 // en pour mille
  3.  
  4. #define PINDIR1 4
  5. #define PINPWM1 5
  6. #define PINDIR2 2
  7. #define PINPWM2 3
  8. #define PINTRIG 12
  9. #define PINECHO 11
  10.  
  11.  
  12.  
  13. /* Constantes pour le timeout */
  14. const unsigned long MEASURE_TIMEOUT = 25000UL; // 25ms = ~8m à 340m/s
  15.  
  16. typedef struct {
  17.   bool far_l, far_r, close_l, close_r;
  18. } IR_Capture;
  19.  
  20. typedef enum { INIT, RUN, LEFT, RIGHT, PAUSE, STOP } State ;
  21. typedef enum { L, R } Direction;
  22. bool line_l, line_r; // ligne sur la gauche/droite (capteur infrarouge)
  23. bool obstacle_f; // obstacle devant (capteur ultrason)
  24. bool turn, turn_l; // y a une proba de tourner au cours de la course (marche aléatoire), et une proba pour savoir si on tourne à gauche ou droite
  25. bool setting_up, fatal_error, success, start;
  26. int speedL=0;
  27. int speedR=0;
  28. double prop =1.100;
  29. bool led=false;
  30. int vit=50;
  31.  
  32. State state;
  33.  
  34. void setup() {
  35.   // port série
  36.   Serial.begin(115200);
  37.     //on initialise les pins du moteur 1
  38.   pinMode(PINDIR1, OUTPUT);
  39.   pinMode(PINPWM1, OUTPUT);
  40.   pinMode(PINDIR2, OUTPUT);
  41.   pinMode(PINPWM2, OUTPUT);
  42.  
  43.   digitalWrite(PINDIR1,LOW);
  44.   digitalWrite(PINDIR2,LOW);
  45.  
  46.   pinMode(PINTRIG, OUTPUT);
  47.   digitalWrite(PINTRIG, LOW); // La broche TRIGGER doit être à LOW au repos
  48.   pinMode(PINECHO, INPUT);
  49.  
  50.   pinMode(LED_BUILTIN, OUTPUT);
  51.   digitalWrite(LED_BUILTIN, LOW);
  52.  
  53.   state=INIT;
  54. }
  55.  
  56. IR_Capture detect_IR()
  57. {
  58.   // capteurs infrarouges
  59.   IR_Capture cap;
  60.  
  61.   return cap;
  62. }
  63.  
  64. void detect_US()
  65. {
  66.   // capteurs ultrasons
  67.   digitalWrite(PINTRIG, HIGH);
  68.   delayMicroseconds(10);
  69.   digitalWrite(PINTRIG, LOW);
  70.   long measure = pulseIn(PINECHO, HIGH, MEASURE_TIMEOUT);
  71.   Serial.println(measure);
  72.   obstacle_f = (measure!=0 && measure<2000);
  73. }
  74.  
  75. void refresh()
  76. {
  77.   // met à jour les globals (conditions)
  78.   detect_IR();
  79.   detect_US();
  80.   turn = (random(1000)<P_TURN);
  81.   turn_l = (random(2)==0);
  82. }
  83.  
  84. void transition()
  85. {
  86.   if (fatal_error || success)
  87.     {
  88.       state = STOP;
  89.       return;
  90.     }
  91.   switch (state)
  92.   {
  93.     case INIT:
  94.       if (setting_up)
  95.       {
  96.         state = INIT;
  97.       }
  98.       else
  99.       {
  100.         if (!obstacle_f && !line_l && !line_r && !turn)
  101.         {
  102.           state = RUN;
  103.         }
  104.         else if (turn && turn_l)
  105.         {
  106.           state = LEFT;
  107.         }
  108.         else if (turn && !turn_l)
  109.         {
  110.           state = RIGHT;
  111.         }
  112.         else
  113.         {
  114.           state = PAUSE;
  115.         }
  116.       }
  117.       break;
  118.     case RUN:
  119.       if (!line_l && !line_r && !turn && !obstacle_f)
  120.       {
  121.         state = RUN;
  122.       }
  123.       else if (line_r || (!line_l && !line_r && turn && turn_l) && !obstacle_f)
  124.       {
  125.         state = LEFT;
  126.       }
  127.       else if (line_l || (!line_l && !line_r && turn && !turn_l) && !obstacle_f)
  128.       {
  129.         state = RIGHT;
  130.       }
  131.       else if (obstacle_f)
  132.       {
  133.         state = PAUSE;
  134.       }
  135.       else
  136.       {
  137.         state = STOP;
  138.       }
  139.       break;
  140.     case LEFT:
  141.       if (!line_l && !line_r && !obstacle_f)
  142.       {
  143.         state = RUN;
  144.       }
  145.       else if (line_l || line_r && !obstacle_f)
  146.       {
  147.         state = LEFT;
  148.       }
  149.       else if (obstacle_f)
  150.       {
  151.         state = PAUSE;
  152.       }
  153.       else
  154.       {
  155.         state = STOP;
  156.       }
  157.       break;
  158.     case RIGHT:
  159.       if (!line_l && !line_r && !obstacle_f)
  160.       {
  161.         state = RUN;
  162.       }
  163.       else if (line_l || line_r && !obstacle_f)
  164.       {
  165.         state = RIGHT;
  166.       }
  167.       else if (obstacle_f)
  168.       {
  169.         state = PAUSE;
  170.       }
  171.       else
  172.       {
  173.         state = STOP;
  174.       }
  175.       break;
  176.     case PAUSE:
  177.       if (!line_l && !line_r && !obstacle_f)
  178.       {
  179.         state = RUN;
  180.       }
  181.       else if (line_r && !obstacle_f)
  182.       {
  183.         state = LEFT;
  184.       }
  185.       else if (line_l && !obstacle_f)
  186.       {
  187.         state = RIGHT;
  188.       }
  189.       else if (obstacle_f)
  190.       {
  191.         state = PAUSE;
  192.       }
  193.       break;
  194.     case STOP:
  195.       if (start)
  196.       {
  197.         state = INIT;
  198.       }
  199.       else
  200.       {
  201.         state = STOP;
  202.       }
  203.       break;
  204.     default:
  205.       state = STOP;
  206.       break;
  207.   }
  208.  
  209. }
  210.  
  211. void loop() {
  212.   // put your main code here, to run repeatedly:
  213.   unsigned long t0=millis();
  214.   refresh();
  215.   transition();
  216.   //vit=(vit+7)%255;
  217.   //speedL=250;
  218.  
  219.   switch (state)
  220.   {
  221.     case INIT:
  222.       Serial.println(F("INIT"));
  223.       state=RUN;
  224.       break;
  225.     case RUN:
  226.       Serial.println(F("RUN"));
  227.       led=true;
  228.       speedL=vit;
  229.       speedR=vit;
  230.       break;
  231.     case LEFT:
  232.       Serial.println(F("LEFT"));
  233.       led=true;
  234.       speedL=0;
  235.       speedR=vit;
  236.       break;
  237.     case RIGHT:
  238.       Serial.println(F("RIGHT"));
  239.       led=true;
  240.       speedL=vit;
  241.       speedR=0;
  242.       break;
  243.     case PAUSE:
  244.       Serial.println(F("PAUSE"));
  245.       led=false;
  246.       speedL=0;
  247.       speedR=0;
  248.       break;
  249.     case STOP:
  250.       Serial.println(F("STOP"));
  251.       led=false;
  252.       speedL=0;
  253.       speedR=0;
  254.       break;
  255.     default:
  256.       break;
  257.   }
  258.  
  259.   analogWrite(PINPWM1,digitalRead(!PINDIR1 )?(int)(prop*speedL):(int)prop*(255-prop*speedL));
  260.   analogWrite(PINPWM2,digitalRead(!PINDIR2 )?speedR:255-speedR);
  261.   digitalWrite(LED_BUILTIN, (led)?HIGH:LOW);
  262.   //Serial.print(F("Current state: "));
  263.   //Serial.println(state);
  264.  
  265.   unsigned long elapsed=millis()-t0;
  266.   if (elapsed>INTERVAL) printf("Excessive execution time\n");
  267.   else delay(INTERVAL-elapsed);
  268.  
  269. }
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!
 
Top