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();
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
const byte ledPIN = 13;
long duration;
int distance;
-const int minDistance = 10;
+const int minDistance = 20;
////////////////////////
// Funzioni:
// 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);
// 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;
}
--- /dev/null
+/* 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
+}
--- /dev/null
+/* 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;
+ }
+}
#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()
{
// 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);
}
// 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);
//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;
projects / rover / commitdiff