git.piffa.net Git - rover/blob - prototypes/macchina/fsm/base_debug/base_debug.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 #define dEBUG
  16
  17 int rotPausa = 500; // Pausa per una rotazione di ~90'
  18
  19 void setup()
  20 {
  21     abilita();
  22 #ifdef DEBUG
  23     Serial.begin(9600);
  24     Serial.println("Attivazione sketch");
  25 #endif
  26 }
  27
  28
  29 // FSM
  30 enum  { // Stati della FMS
  31     forward,
  32     check,        // Cerca percorso libero
  33     sx,       // sx
  34     dx,        // dx
  35     ferma,
  36     inversione
  37 } stato  = forward;
  38
  39 const int giroPausa = 630 ; // Tempo necessario per rotazione
  40 void loop()
  41 {
  42     switch (stato) {
  43     case forward:
  44         servoMiddle();
  45         avanti();
  46         delay(10); // Movimento minimo, per stabilizzare
  47         // l'input del sensore
  48         if (distanceCheck()) {
  49 #ifdef DEBUG
  50 Serial.println("\t ###### Ostacolo! ######");
  51 #endif
  52             stato = check ;
  53         }
  54
  55         break;
  56
  57     case check:
  58         if (!distanceCheck()) {
  59             stato = forward ;
  60             break;
  61         }
  62         stop();
  63 // check dx
  64         servoDX();
  65         if (!distanceCheck()) {
  66             stato = dx ;
  67             servoMiddle();
  68             break;
  69         }
  70
  71 // check sx
  72         servoSX();
  73         if (!distanceCheck()) {
  74             stato = sx ;
  75             servoMiddle();
  76             break;
  77         }
  78 // Inversione
  79         servoMiddle();
  80         stato = inversione ;
  81 #ifdef DEBUG
  82 Serial.print("Stato: ");
  83 Serial.println(stato);
  84 #endif
  85         break;
  86
  87     case sx:
  88 #ifdef DEBUG
  89 Serial.print("Stato: ");
  90 Serial.println(stato);
  91 #endif
  92         giraSX();
  93         delay(giroPausa);
  94             stato = forward ;
  95         break;
  96
  97     case dx:
  98 #ifdef DEBUG
  99 Serial.print("Stato: ");
 100 Serial.println(stato);
 101 #endif
 102         giraDX();
 103         delay(giroPausa);
 104             stato = forward ;
 105         break;
 106
 107     case ferma:
 108         stop();
 109         delay(1000);
 110         stato = check ;
 111         break;
 112
 113     case inversione:
 114 #ifdef DEBUG
 115 Serial.print("Stato: ");
 116 Serial.println(stato);
 117 #endif
 118         stop();
 119         indietro(); // Why not? :)
 120         delay(giroPausa * 1);
 121         giraDX();
 122         delay(giroPausa * 3);
 123         stato = check ;
 124         break;
 125
 126     default:
 127         stop();
 128         delay(2000);
 129         stato = check ;
 130         break;
 131     }
 132
 133 #ifdef DEBUG
 134 //Serial.print("Stato: ");
 135 //Serial.println(stato);
 136 //Serial.print("Distanza: ");
 137 //Serial.println(distanceMonitor());
 138 #endif
 139 }