git.piffa.net Git - rover/blob - prototypes/macchina/fsm/simple/simple.ino

   1 /* FSM  Base
   2
   3 Prototipo rozzo per il movimento utilizzando le funzioni
   4 di base della libreria Rover e una FSM.
   5
   6
   7 Schema: https://lab.piffa.net/schemi/2wd_car_bb.png
   8 Codice: http://git.andreamanni.com/web?p=rove
   9
  10 */
  11
  12
  13 #include "rover.h"
  14 #include "Servo.h"
  15
  16 int rotPausa = 500; // Pausa per una rotazione di ~90'
  17
  18 void setup()
  19 {
  20     abilita();
  21     servoMiddle();
  22 }
  23
  24
  25 // FSM
  26 enum  { // Stati della FMS
  27     forward,
  28     check,        // Cerca percorso libero
  29     sx,       // sx
  30     dx,        // dx
  31     ferma,
  32     inversione
  33 } stato  = forward;
  34
  35 const int giroPausa = 500 ; // Tempo necessario per rotazione
  36 void loop()
  37 {
  38     switch (stato) {
  39     case forward:
  40         servoMiddle();
  41         avanti();
  42         delay(50); // Movimento minimo, per stabilizzare
  43         // l'input del sensore
  44         if (distanceCheck()) {
  45             stato == check ;
  46         }
  47
  48         break;
  49
  50     case check:
  51         if (!distanceCheck()) {
  52             stato == forward ;
  53             break;
  54         }
  55         stop();
  56 // check dx
  57         servoDX();
  58         if (!distanceCheck()) {
  59             stato == dx ;
  60             servoMiddle();
  61             break;
  62         }
  63
  64 // check sx
  65         servoSX();
  66         if (!distanceCheck()) {
  67             stato == sx ;
  68             servoMiddle();
  69             break;
  70         }
  71 // Inversione
  72         servoMiddle();
  73         stato == inversione ;
  74         break;
  75
  76     case sx:
  77         giraSX();
  78         delay(giroPausa);
  79         break;
  80
  81     case dx:
  82         giraDX();
  83         delay(giroPausa);
  84         break;
  85
  86     case ferma:
  87         stop();
  88         delay(1000);
  89         stato == check ;
  90         break;
  91
  92     case inversione:
  93         stop();
  94         indietro(); // Why not? :)
  95         delay(1000);
  96         delay(giroPausa * 2);
  97         stato == check ;
  98         break;
  99
 100     default:
 101         stop();
 102         delay(2000);
 103         stato == check ;
 104         break;
 105     }
 106 }