Prototipo FSM

diff --git a/libraries/rover/examples/drive/drive.ino b/libraries/rover/examples/drive/drive.ino
index c50da6229bd9b0a329423acdb7182e6cff362299..fbe868cf48ad1a14c87465a699c472abd5a46ffb 100644 (file)
--- a/libraries/rover/examples/drive/drive.ino
+++ b/libraries/rover/examples/drive/drive.ino
@@ -19,27 +19,37 @@ void setup() {
 
 
 void loop() {
 
 
 void loop() {

+digitalWrite(13,HIGH);
+

 // Avanti
     avanti();
     delay(2000);
 // Avanti
     avanti();
     delay(2000);

+
+digitalWrite(13,LOW);
+

 // Stop
     stop();
     delay(1000);
 
 // TurnSX
     giraSX();
 // Stop
     stop();
     delay(1000);
 
 // TurnSX
     giraSX();

+    delay(2000);
+
+// Stop
+    stop();

     delay(1000);
 
     delay(1000);
 

-// Avanti
-    avanti();
+// indietro
+    indietro();

     delay(2000);
     delay(2000);

+

 // Stop
     stop();
     delay(1000);
 
 // TurnDX
     giraDX();
 // Stop
     stop();
     delay(1000);
 
 // TurnDX
     giraDX();

-    delay(1000);
+    delay(2000);

 
 // Stop
     stop();
 
 // Stop
     stop();

diff --git a/libraries/rover/rover.cpp b/libraries/rover/rover.cpp
index a5989b6dd44f0cf1a127f9bd2ed97df4d8eb1e55..61d8e86fb98d9c0b73ff001acc0833211a266c41 100644 (file)
--- a/libraries/rover/rover.cpp
+++ b/libraries/rover/rover.cpp
@@ -18,20 +18,20 @@ Licenza:    GPLv3
 const int enA = 6;
 const int in1 = 7;
 const int in2 = 8;
 const int enA = 6;
 const int in1 = 7;
 const int in2 = 8;

-byte speedA = 255;
+byte speedA = 120; // DX

 // motor two
 const int enB = 5;
 const int in3 = 4;
 const int in4 = 3;
 // motor two
 const int enB = 5;
 const int in3 = 4;
 const int in4 = 3;

-byte speedB = 255;
+byte speedB = 100;

 
 // Servo vars
 int pos = 0;    // variable to store the servo position
 const byte servoPIN =9 ;
 const byte middle = 90; // Centratura servo
 const int spausa = 10; // Pausa movimenti servo
 
 // Servo vars
 int pos = 0;    // variable to store the servo position
 const byte servoPIN =9 ;
 const byte middle = 90; // Centratura servo
 const int spausa = 10; // Pausa movimenti servo

-const byte sx = 10;  // Min SX
-const byte dx = 170; // Maz DX
+const byte sx = 30;  // Min SX
+const byte dx = 150; // Maz DX

 Servo myservo; // Non c'e' bisogno di extern se e' dichiarato in questo scope
 
 // Ultrasuoni
 Servo myservo; // Non c'e' bisogno di extern se e' dichiarato in questo scope
 
 // Ultrasuoni


@@ -40,7 +40,7 @@ const byte echoPIN= 12;
 const byte ledPIN = 13;
 long duration;
 int distance;
 const byte ledPIN = 13;
 long duration;
 int distance;

-const int minDistance = 10;
+const int minDistance = 20;

 
 ////////////////////////
 // Funzioni:
 
 ////////////////////////
 // Funzioni:


@@ -204,6 +204,7 @@ boolean distanceCheck() {
     // so it takes ~29.1 milliseconds for a cm.
     distance = (duration / 58.2); // Atmegas are not found of divisions
     // Distance is half of (out + back)
     // so it takes ~29.1 milliseconds for a cm.
     distance = (duration / 58.2); // Atmegas are not found of divisions
     // Distance is half of (out + back)

+    distance = constrain(distance,4,35);

 #ifdef DEBUG
 Serial.print("Distanza oggetto: ");    
 Serial.println(distance);
 #ifdef DEBUG
 Serial.print("Distanza oggetto: ");    
 Serial.println(distance);


@@ -228,6 +229,7 @@ int distanceMonitor() {
     // so it takes ~29.1 milliseconds for a cm.
     distance = (duration / 58.2); // Atmegas are not found of divisions
     // Distance is half of (out + back)
     // so it takes ~29.1 milliseconds for a cm.
     distance = (duration / 58.2); // Atmegas are not found of divisions
     // Distance is half of (out + back)

+    distance = constrain(distance,4,35);

 
     return distance;
 }
 
     return distance;
 }

diff --git a/prototypes/macchina/fsm/base/base.ino b/prototypes/macchina/fsm/base/base.ino
new file mode 100644 (file)
index 0000000..2ac12cb
--- /dev/null
+++ b/prototypes/macchina/fsm/base/base.ino
@@ -0,0 +1,139 @@
+/* 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"
+#define dEBUG
+
+int rotPausa = 500; // Pausa per una rotazione di ~90'
+
+void setup()
+{
+    abilita();
+#ifdef DEBUG
+    Serial.begin(9600);
+    Serial.println("Attivazione sketch");
+#endif
+}
+
+
+// FSM
+enum  { // Stati della FMS
+    forward,
+    check,       // Cerca percorso libero
+    sx,       // sx
+    dx,        // dx
+    ferma,
+    inversione
+} stato  = forward;
+
+const int giroPausa = 630 ; // Tempo necessario per rotazione
+void loop()
+{
+    switch (stato) {
+    case forward:
+        servoMiddle();
+        avanti();
+        delay(10); // Movimento minimo, per stabilizzare
+        // l'input del sensore
+        if (distanceCheck()) {
+#ifdef DEBUG
+Serial.println("\t ###### Ostacolo! ######");       
+#endif
+            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 ;
+#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 ;
+        break;
+
+    case inversione:
+#ifdef DEBUG
+Serial.print("Stato: ");       
+Serial.println(stato);       
+#endif
+        stop();
+        indietro(); // Why not? :)
+        delay(giroPausa * 1);
+        giraDX();
+        delay(giroPausa * 3);
+        stato = check ;
+        break;
+
+    default:
+        stop();
+        delay(2000);
+        stato = check ;
+        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/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;
+    }
+}

diff --git a/prototypes/macchina/raw/raw.ino b/prototypes/macchina/raw/raw.ino
index 3261b98f49d9b99af4d2e6a639ad9a10646a46ca..76c78c1f053f78d6168ec82a3ae7ffef2a5a4344 100644 (file)
--- a/prototypes/macchina/raw/raw.ino
+++ b/prototypes/macchina/raw/raw.ino
@@ -13,7 +13,7 @@ Codice: http://git.andreamanni.com/web?p=rove
 #include "rover.h"
 #include "Servo.h"
 
 #include "rover.h"
 #include "Servo.h"
 

-int rotPausa = 1000; // Pausa per una rotazione di ~90'
+int rotPausa = 200; // Pausa per una minima rotazione 

 
 void setup()
 {
 
 void setup()
 {


@@ -27,9 +27,10 @@ void loop()
 // Fintanto che non ci sono ostacoli: avanti
     while (distanceCheck() == 0) {
         avanti();
 // Fintanto che non ci sono ostacoli: avanti
     while (distanceCheck() == 0) {
         avanti();

+        delay(50); // Movimento minimo, per stabilizzare
+        // l'input del sensore

     }
     }

-// Se c'e' un ostacolo: ferma e gira
-    stop();
-    giraSX;
+// Se c'e' un ostacolo: gira
+    giraDX();

     delay(rotPausa);
 }
     delay(rotPausa);
 }

diff --git a/prototypes/ultra/check_func/check_func.ino b/prototypes/ultra/check_func/check_func.ino
index 815e3527a19922d17a2df47db48b9366a690493a..057f17e2f61d2db5ba6830aa3c8a970fd12b2934 100644 (file)
--- a/prototypes/ultra/check_func/check_func.ino
+++ b/prototypes/ultra/check_func/check_func.ino
@@ -52,6 +52,7 @@ boolean distanceCheck() {
     // so it takes ~29.1 milliseconds for a cm.
     distance = (duration / 58.2); // Atmegas are not found of divisions
     // Distance is half of (out + back)
     // so it takes ~29.1 milliseconds for a cm.
     distance = (duration / 58.2); // Atmegas are not found of divisions
     // Distance is half of (out + back)

+    distance = constrain(distance,4,35);

 #ifdef DEBUG
 Serial.print("Distanza oggetto: ");    
 Serial.println(distance);
 #ifdef DEBUG
 Serial.print("Distanza oggetto: ");    
 Serial.println(distance);


@@ -74,6 +75,7 @@ int distanceMonitor() {
     //distance = (duration / 2) / 29.1; // Speed is ~300m/s,
     // so it takes ~29.1 milliseconds for a cm.
     distance = (duration / 58.2); // Atmegas are not found of divisions
     //distance = (duration / 2) / 29.1; // Speed is ~300m/s,
     // so it takes ~29.1 milliseconds for a cm.
     distance = (duration / 58.2); // Atmegas are not found of divisions

+    distance = constrain(distance,4,35);

     // Distance is half of (out + back)
 
     return distance;
     // Distance is half of (out + back)
 
     return distance;