initial commit
initial commit

file:b/rover/rover.ino (new)
--- /dev/null
+++ b/rover/rover.ino
@@ -1,1 +1,171 @@
+/**
+* @author Razvan Stanga
+* 
+**/
 
+#include <UltrasonicInterrupt.h>
+#include <Servo.h> 
+
+int DEBUG=1;
+int ODEBUG=DEBUG;
+
+const float SpeedOfSound     	= 343.2; // ~speed of sound (m/s) in air, at 20°C
+const float MicrosecondsPerMillimetre 	= 1000.0 / SpeedOfSound; // microseconds per millimetre - sound travels 1 mm in ~2.9us
+
+const float MicrosecondsToMillimetres  = (1.0 / MicrosecondsPerMillimetre);
+const float MicrosecondsToMillimetres2 = MicrosecondsToMillimetres / 2.0; // beam travels the distance twice... so halve the time.
+
+unsigned long lastTime	= 0;
+
+int pingTimer			= 0;
+int pingDelay			= 300; // milliseconds between ping pulses
+int debugTimer			= 0;
+int debugDelay			= 300;
+
+UltrasonicInterrupt s1;
+const int SIGNAL1_PIN	= 2; 	// digital pin connected to the trigger port on the module
+const int INTERRUPT1  = 2;    // interrupt
+const int INTERRUPT1_PIN  = 0;  // custom interrupt pin, see above
+float millimetres_s1 		= 0.0;
+
+Servo FL; int FLPIN = 3; int FLC = 86;
+Servo FR; int FRPIN = 5; int FRC = 92;
+Servo BL; int BLPIN = 9; int BLC = 93;
+Servo BR; int BRPIN = 6; int BRC = 93;
+
+int forwardspeed = 10;
+int steerspeed = 10;
+int backspeed = 5;
+bool forward = true;
+bool steer = true;
+bool back = true;
+
+void setup() {
+
+  if (DEBUG) {
+    Serial.begin(9600);
+  }
+
+  // start s1 sensor instance
+  s1.setup(INTERRUPT1, INTERRUPT1_PIN, SIGNAL1_PIN, callback_s1);
+  s1.begin();
+  
+  FL.attach(FLPIN);
+  FL.write (FLC);
+  FR.attach(FRPIN);
+  FR.write (FRC);
+  BL.attach(BLPIN);
+  BL.write (BLC);
+  BR.attach(BRPIN);
+  BR.write (BRC);
+}
+
+void loop() {
+  
+  unsigned long time = millis();
+  unsigned long dt   = time - lastTime;
+  lastTime 	     = time;
+  
+  pingTimer += dt;
+  debugTimer += dt;
+  if(pingTimer > pingDelay){
+	pingTimer = 0;
+        s1.ping();
+  }
+  
+  // do stuff...  
+  if ( millimetres_s1 < 400 ) {
+    if ( millimetres_s1 < 250 ) {
+      if (DEBUG>=1) {
+        Serial.println ("obstacle ahead in under 250mm, goback");
+      }
+      unsigned long backTimer = millis() + 500;
+      while ( millis() < backTimer ) {
+        if (back) {
+          goback();
+          back = false;
+        }
+      }
+      goright();
+      forward = true; steer = true;
+    } else {
+      if (DEBUG>=1) {
+        Serial.println ("obstacle ahead in under 400mm, goright");
+      }
+      if (steer) {
+        goright ();
+        forward = true; back = true; steer = false;
+      }
+    }
+  } else {
+    if (DEBUG>=1) {
+      Serial.println ("no obstacle ahead, goforward");
+    }    
+    if (forward) {
+      goforward();
+      steer = true; back = true; forward = false;
+    }
+  }
+  
+  if(debugTimer > debugDelay){
+    debugTimer = 0;
+    DEBUG = ODEBUG;
+  } else {
+    DEBUG = 0;
+  }
+}
+
+void stop () {
+  FL.write (FLC);
+  FR.write (FRC);
+  BL.write (BLC);
+  BR.write (BRC);
+}
+
+void goright () {
+  FL.write (FLC+steerspeed);
+  FR.write (FRC+steerspeed);
+  BL.write (BLC+steerspeed);
+  BR.write (BRC-steerspeed);
+}
+
+void goforward () {
+  FL.write (FLC+forwardspeed);
+  FR.write (FRC-forwardspeed);
+  BL.write (BLC+forwardspeed);
+  BR.write (BRC+forwardspeed);
+}
+
+void goback () {
+  FL.write (FLC-backspeed);
+  FR.write (FRC+backspeed);
+  BL.write (BLC-backspeed);
+  BR.write (BRC-backspeed);
+}
+
+/**
+* Pulse complete callback handler for UltrasonicInterrupt s1 instance
+* @param duration - pulse length in microseconds
+*/
+void callback_s1(unsigned long duration) {
+
+  millimetres_s1 = MicrosecondsToMillimetres2 * duration;
+
+  if(millimetres_s1 > 4000){
+    if (DEBUG>=2){
+      Serial.println("s1 out of range");
+    }
+    // out of range (http://users.ece.utexas.edu/~valvano/Datasheets/HCSR04b.pdf)
+    millimetres_s1 = 4000;
+  } else {
+    if (DEBUG>=2){
+      Serial.print("s1 duration: ");
+      Serial.print(duration);
+      Serial.print(" us, distance: ");
+      Serial.print(millimetres_s1);
+      Serial.println(" mm");
+    }
+  }
+}
+

+

comments