From ab995024225af895d46730bc34d758978e24f9e5 Mon Sep 17 00:00:00 2001 From: Andrea Manni Date: Thu, 6 Apr 2017 16:56:10 +0200 Subject: [PATCH] Prototipo FSM --- libraries/rover/examples/drive/drive.ino | 16 ++- libraries/rover/rover.cpp | 12 +- prototypes/macchina/fsm/base/base.ino | 139 +++++++++++++++++++++ prototypes/macchina/fsm/simple/simple.ino | 106 ++++++++++++++++ prototypes/macchina/raw/raw.ino | 9 +- prototypes/ultra/check_func/check_func.ino | 2 + 6 files changed, 272 insertions(+), 12 deletions(-) create mode 100644 prototypes/macchina/fsm/base/base.ino create mode 100644 prototypes/macchina/fsm/simple/simple.ino diff --git a/libraries/rover/examples/drive/drive.ino b/libraries/rover/examples/drive/drive.ino index c50da62..fbe868c 100644 --- a/libraries/rover/examples/drive/drive.ino +++ b/libraries/rover/examples/drive/drive.ino @@ -19,27 +19,37 @@ void setup() { void loop() { +digitalWrite(13,HIGH); + // Avanti avanti(); delay(2000); + +digitalWrite(13,LOW); + // Stop stop(); delay(1000); // TurnSX giraSX(); + delay(2000); + +// Stop + stop(); delay(1000); -// Avanti - avanti(); +// indietro + indietro(); delay(2000); + // Stop stop(); delay(1000); // TurnDX giraDX(); - delay(1000); + delay(2000); // Stop stop(); diff --git a/libraries/rover/rover.cpp b/libraries/rover/rover.cpp index a5989b6..61d8e86 100644 --- 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; -byte speedA = 255; +byte speedA = 120; // DX // 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 -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 @@ -40,7 +40,7 @@ const byte echoPIN= 12; const byte ledPIN = 13; long duration; int distance; -const int minDistance = 10; +const int minDistance = 20; //////////////////////// // 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) + distance = constrain(distance,4,35); #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) + distance = constrain(distance,4,35); return distance; } diff --git a/prototypes/macchina/fsm/base/base.ino b/prototypes/macchina/fsm/base/base.ino new file mode 100644 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 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 3261b98..76c78c1 100644 --- 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" -int rotPausa = 1000; // Pausa per una rotazione di ~90' +int rotPausa = 200; // Pausa per una minima rotazione void setup() { @@ -27,9 +27,10 @@ void loop() // 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); } diff --git a/prototypes/ultra/check_func/check_func.ino b/prototypes/ultra/check_func/check_func.ino index 815e352..057f17e 100644 --- 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) + distance = constrain(distance,4,35); #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 = constrain(distance,4,35); // Distance is half of (out + back) return distance; -- 2.39.2