Prototipo FSM

[rover] / prototypes / macchina / fsm / simple / simple.ino

diff --git a/prototypes/macchina/fsm/simple/simple.ino b/prototypes/macchina/fsm/simple/simple.ino
new file mode 100644 (file)
index 0000000..ad16c4b
--- /dev/null
+++ b/prototypes/macchina/fsm/simple/simple.ino
@@ -0,0 +1,106 @@
+/* FSM  Base
+
+Prototipo rozzo per il movimento utilizzando le funzioni
+di base della libreria Rover e una FSM.
+
+
+Schema: https://lab.piffa.net/schemi/2wd_car_bb.png
+Codice: http://git.andreamanni.com/web?p=rove
+
+*/
+
+
+#include "rover.h"
+#include "Servo.h"
+
+int rotPausa = 500; // Pausa per una rotazione di ~90'
+
+void setup()
+{
+    abilita();
+    servoMiddle();
+}
+
+
+// FSM
+enum  { // Stati della FMS
+    forward,
+    check,       // Cerca percorso libero
+    sx,       // sx
+    dx,        // dx
+    ferma,
+    inversione
+} stato  = forward;
+
+const int giroPausa = 500 ; // Tempo necessario per rotazione
+void loop()
+{
+    switch (stato) {
+    case forward:
+        servoMiddle();
+        avanti();
+        delay(50); // Movimento minimo, per stabilizzare
+        // l'input del sensore
+        if (distanceCheck()) {
+            stato == check ;
+        }
+
+        break;
+
+    case check:
+        if (!distanceCheck()) {
+            stato == forward ;
+            break;
+        }
+        stop();
+// check dx
+        servoDX();
+        if (!distanceCheck()) {
+            stato == dx ;
+            servoMiddle();
+            break;
+        }
+
+// check sx
+        servoSX();
+        if (!distanceCheck()) {
+            stato == sx ;
+            servoMiddle();
+            break;
+        }
+// Inversione
+        servoMiddle();
+        stato == inversione ;
+        break;
+
+    case sx:
+        giraSX();
+        delay(giroPausa);
+        break;
+
+    case dx:
+        giraDX();
+        delay(giroPausa);
+        break;
+
+    case ferma:
+        stop();
+        delay(1000);
+        stato == check ;
+        break;
+
+    case inversione:
+        stop();
+        indietro(); // Why not? :)
+        delay(1000);
+        delay(giroPausa * 2);
+        stato == check ;
+        break;
+
+    default:
+        stop();
+        delay(2000);
+        stato == check ;
+        break;
+    }
+}