git.piffa.net Git - rover/blob - prototypes/macchina/fsm/base/base.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 }
  22
  23
  24 // FSM
  25 enum  { // Stati della FMS
  26     forward,
  27     check,        // Cerca percorso libero
  28     sx,       // sx
  29     dx,        // dx
  30     ferma,
  31     inversione
  32 } stato  = forward;
  33
  34 const int giroPausa = 630 ; // Tempo necessario per rotazione
  35 void loop()
  36 {
  37     switch (stato) {
  38     case forward:
  39         servoMiddle();
  40         avanti();
  41         delay(10); // Movimento minimo, per stabilizzare
  42         // l'input del sensore
  43         if (distanceCheck()) {
  44             stato = check ;
  45         }
  46
  47         break;
  48
  49     case check:
  50         if (!distanceCheck()) {
  51             stato = forward ;
  52             break;
  53         }
  54         stop();
  55 // check dx
  56         servoDX();
  57         if (!distanceCheck()) {
  58             stato = dx ;
  59             servoMiddle();
  60             break;
  61         }
  62
  63 // check sx
  64         servoSX();
  65         if (!distanceCheck()) {
  66             stato = sx ;
  67             servoMiddle();
  68             break;
  69         }
  70 // Inversione
  71         servoMiddle();
  72         stato = inversione ;
  73         break;
  74
  75     case sx:
  76         giraSX();
  77         delay(giroPausa);
  78             stato = forward ;
  79         break;
  80
  81     case dx:
  82         giraDX();
  83         delay(giroPausa);
  84             stato = forward ;
  85         break;
  86
  87     case ferma:
  88         stop();
  89         delay(1000);
  90         stato = check ;
  91         break;
  92
  93     case inversione:
  94         stop();
  95         indietro(); // Why not? :)
  96         delay(giroPausa * 1);
  97         giraDX();
  98         delay(giroPausa * 3);
  99         stato = check ;
 100         break;
 101
 102     default:
 103         stop();
 104         delay(2000);
 105         stato = check ;
 106         break;
 107     }
 108
 109 }