From 24b5e111ad1899efc45338275e7380536382aced Mon Sep 17 00:00:00 2001
From: Andrea Manni <andrea@piffa.net>
Date: Thu, 6 Apr 2017 17:14:17 +0200
Subject: [PATCH] Cleanup

---
 prototypes/macchina/fsm/base/base.ino         | 30 ----------
 .../simple.ino => base_debug/base_debug.ino}  | 59 +++++++++++++++----
 2 files changed, 46 insertions(+), 43 deletions(-)
 rename prototypes/macchina/fsm/{simple/simple.ino => base_debug/base_debug.ino} (56%)

diff --git a/prototypes/macchina/fsm/base/base.ino b/prototypes/macchina/fsm/base/base.ino
index 2ac12cb..4dada11 100644
--- a/prototypes/macchina/fsm/base/base.ino
+++ b/prototypes/macchina/fsm/base/base.ino
@@ -12,17 +12,12 @@ Codice: http://git.andreamanni.com/web?p=rove
 
 #include "rover.h"
 #include "Servo.h"
-#define dEBUG
 
 int rotPausa = 500; // Pausa per una rotazione di ~90'
 
 void setup()
 {
     abilita();
-#ifdef DEBUG
-    Serial.begin(9600);
-    Serial.println("Attivazione sketch");
-#endif
 }
 
 
@@ -46,9 +41,6 @@ void loop()
         delay(10); // Movimento minimo, per stabilizzare
         // l'input del sensore
         if (distanceCheck()) {
-#ifdef DEBUG
-Serial.println("\t ###### Ostacolo! ######");       
-#endif
             stato = check ;
         }
 
@@ -78,27 +70,15 @@ Serial.println("\t ###### Ostacolo! ######");
 // Inversione
         servoMiddle();
         stato = inversione ;
-#ifdef DEBUG
-Serial.print("Stato: ");       
-Serial.println(stato);       
-#endif
         break;
 
     case sx:
-#ifdef DEBUG
-Serial.print("Stato: ");       
-Serial.println(stato);       
-#endif
         giraSX();
         delay(giroPausa);
             stato = forward ;
         break;
 
     case dx:
-#ifdef DEBUG
-Serial.print("Stato: ");       
-Serial.println(stato);       
-#endif
         giraDX();
         delay(giroPausa);
             stato = forward ;
@@ -111,10 +91,6 @@ Serial.println(stato);
         break;
 
     case inversione:
-#ifdef DEBUG
-Serial.print("Stato: ");       
-Serial.println(stato);       
-#endif
         stop();
         indietro(); // Why not? :)
         delay(giroPausa * 1);
@@ -130,10 +106,4 @@ Serial.println(stato);
         break;
     }
 
-#ifdef DEBUG
-//Serial.print("Stato: ");       
-//Serial.println(stato);       
-//Serial.print("Distanza: ");       
-//Serial.println(distanceMonitor());       
-#endif
 }
diff --git a/prototypes/macchina/fsm/simple/simple.ino b/prototypes/macchina/fsm/base_debug/base_debug.ino
similarity index 56%
rename from prototypes/macchina/fsm/simple/simple.ino
rename to prototypes/macchina/fsm/base_debug/base_debug.ino
index ad16c4b..2ac12cb 100644
--- a/prototypes/macchina/fsm/simple/simple.ino
+++ b/prototypes/macchina/fsm/base_debug/base_debug.ino
@@ -12,13 +12,17 @@ Codice: http://git.andreamanni.com/web?p=rove
 
 #include "rover.h"
 #include "Servo.h"
+#define dEBUG
 
 int rotPausa = 500; // Pausa per una rotazione di ~90'
 
 void setup()
 {
     abilita();
-    servoMiddle();
+#ifdef DEBUG
+    Serial.begin(9600);
+    Serial.println("Attivazione sketch");
+#endif
 }
 
 
@@ -32,31 +36,34 @@ enum  { // Stati della FMS
     inversione
 } stato  = forward;
 
-const int giroPausa = 500 ; // Tempo necessario per rotazione
+const int giroPausa = 630 ; // Tempo necessario per rotazione
 void loop()
 {
     switch (stato) {
     case forward:
         servoMiddle();
         avanti();
-        delay(50); // Movimento minimo, per stabilizzare
+        delay(10); // Movimento minimo, per stabilizzare
         // l'input del sensore
         if (distanceCheck()) {
-            stato == check ;
+#ifdef DEBUG
+Serial.println("\t ###### Ostacolo! ######");       
+#endif
+            stato = check ;
         }
 
         break;
 
     case check:
         if (!distanceCheck()) {
-            stato == forward ;
+            stato = forward ;
             break;
         }
         stop();
 // check dx
         servoDX();
         if (!distanceCheck()) {
-            stato == dx ;
+            stato = dx ;
             servoMiddle();
             break;
         }
@@ -64,43 +71,69 @@ void loop()
 // check sx
         servoSX();
         if (!distanceCheck()) {
-            stato == sx ;
+            stato = sx ;
             servoMiddle();
             break;
         }
 // Inversione
         servoMiddle();
-        stato == inversione ;
+        stato = inversione ;
+#ifdef DEBUG
+Serial.print("Stato: ");       
+Serial.println(stato);       
+#endif
         break;
 
     case sx:
+#ifdef DEBUG
+Serial.print("Stato: ");       
+Serial.println(stato);       
+#endif
         giraSX();
         delay(giroPausa);
+            stato = forward ;
         break;
 
     case dx:
+#ifdef DEBUG
+Serial.print("Stato: ");       
+Serial.println(stato);       
+#endif
         giraDX();
         delay(giroPausa);
+            stato = forward ;
         break;
 
     case ferma:
         stop();
         delay(1000);
-        stato == check ;
+        stato = check ;
         break;
 
     case inversione:
+#ifdef DEBUG
+Serial.print("Stato: ");       
+Serial.println(stato);       
+#endif
         stop();
         indietro(); // Why not? :)
-        delay(1000);
-        delay(giroPausa * 2);
-        stato == check ;
+        delay(giroPausa * 1);
+        giraDX();
+        delay(giroPausa * 3);
+        stato = check ;
         break;
 
     default:
         stop();
         delay(2000);
-        stato == check ;
+        stato = check ;
         break;
     }
+
+#ifdef DEBUG
+//Serial.print("Stato: ");       
+//Serial.println(stato);       
+//Serial.print("Distanza: ");       
+//Serial.println(distanceMonitor());       
+#endif
 }
-- 
2.39.2