Prototipo FSM

[rover] / libraries / rover / rover.cpp

diff --git a/libraries/rover/rover.cpp b/libraries/rover/rover.cpp
index 58bbf2cbb082a1f3e2b2c685f8ff1e675df8e311..61d8e86fb98d9c0b73ff001acc0833211a266c41 100644 (file)
--- a/libraries/rover/rover.cpp
+++ b/libraries/rover/rover.cpp
@@ -13,42 +13,54 @@ Licenza:    GPLv3
 
 #define dEBUG
 
-// Configurazione con OUTPUT digitali
+// Configurazione parametri: variabili GLOBALI
 // motor one
 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 trigPIN = 11;
+const byte echoPIN= 12;
+const byte ledPIN = 13;
+long duration;
+int distance;
+const int minDistance = 20;
+
 ////////////////////////
 // Funzioni:
 //
 void abilita() {
-// Abilita i PINs come OUTPUTS
+// Abilita i PINs come OUTPUTS e attacca un servo 
+// Motors
     pinMode(enA, OUTPUT);
     pinMode(in1, OUTPUT);
     pinMode(in2, OUTPUT);
     pinMode(enB, OUTPUT);
     pinMode(in3, OUTPUT);
     pinMode(in4, OUTPUT);
-
-
+// Servo
     pinMode(servoPIN, OUTPUT);
-    myservo.attach(servoPIN);
+    myservo.attach(servoPIN); //la libreria servo deve essere sempre inclusa
+// Ultrasonic
+       pinMode(trigPIN, OUTPUT);
+    pinMode(echoPIN, INPUT);
+    pinMode(ledPIN, OUTPUT);
 }
 
 
@@ -177,3 +189,47 @@ void servoMiddle() {
         delay(spausa);
     }
 }
+
+
+// Ultrasuoni
+boolean distanceCheck() {
+// Verifica se la distanza di un oggetto e' minore di minDistance
+    digitalWrite(trigPIN, LOW);  // Prepare for ping
+    delayMicroseconds(2); //
+    digitalWrite(trigPIN, HIGH); // Send a ping
+    delayMicroseconds(10); //
+    digitalWrite(trigPIN, LOW); // Set down ping
+    duration = pulseIn(echoPIN, HIGH);
+    //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 is half of (out + back)
+    distance = constrain(distance,4,35);
+#ifdef DEBUG
+Serial.print("Distanza oggetto: ");    
+Serial.println(distance);
+#endif
+    if (distance < minDistance) {  // This is where the LED On/Off happens
+        return 1;
+    }
+    else {
+        return 0;
+    }
+}
+
+int distanceMonitor() {
+// Ritorna la distanza di un oggetto in cm
+    digitalWrite(trigPIN, LOW);  // Prepare for ping
+    delayMicroseconds(2); //
+    digitalWrite(trigPIN, HIGH); // Send a ping
+    delayMicroseconds(10); //
+    digitalWrite(trigPIN, LOW); // Set down ping
+    duration = pulseIn(echoPIN, HIGH);
+    //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 is half of (out + back)
+    distance = constrain(distance,4,35);
+
+    return distance;
+}