added servo for ultrasonic sensor for better range master
added servo for ultrasonic sensor for better range
improved movements

--- a/rover/rover.ino
+++ b/rover/rover.ino
@@ -9,7 +9,7 @@
 int DEBUG=1;
 int ODEBUG=DEBUG;
 
-const float SpeedOfSound     	= 343.2; // ~speed of sound (m/s) in air, at 20°C
+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);
@@ -20,7 +20,9 @@
 int pingTimer			= 0;
 int pingDelay			= 300; // milliseconds between ping pulses
 int debugTimer			= 0;
-int debugDelay			= 300;
+int debugDelay			= 10;
+int servoTimer			= 0;
+int servoDelay			= 1000;
 
 UltrasonicInterrupt s1;
 const int SIGNAL1_PIN	= 2; 	// digital pin connected to the trigger port on the module
@@ -32,13 +34,16 @@
 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;
+Servo FS; int FSPIN = 11; int FSC = 90; int FST = FSC; int FSM = 1;
+
+int forwardspeed = 20;
 int steerspeed = 10;
-int backspeed = 5;
+int backspeed = 10;
 bool forward = true;
-bool steer = true;
-bool back = true;
+bool steer = false;
+bool back = false;
+
+int deg;
 
 void setup() {
 
@@ -58,6 +63,12 @@
   BL.write (BLC);
   BR.attach(BRPIN);
   BR.write (BRC);
+  
+  FS.attach(FSPIN);
+  FS.write (FSC);
+  
+  delay (3000);
+  Serial.println ("------- start rover --------");
 }
 
 void loop() {
@@ -68,50 +79,76 @@
   
   pingTimer += dt;
   debugTimer += dt;
+  servoTimer += 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;
-    }
-  }
-  
+    pingTimer = 0;
+    s1.ping();
+  }
+
   if(debugTimer > debugDelay){
     debugTimer = 0;
-    DEBUG = ODEBUG;
-  } else {
-    DEBUG = 0;
+    //DEBUG = ODEBUG;
+  } else {
+    //DEBUG = 0;
+  }
+
+  // do stuff...
+  if (FSM==1) {
+    int i;
+    for (i=40;i<=140;i++) {
+      FS.write (i);
+      delay (10);
+      s1.ping();
+      moverover(i);
+    }
+    for (i=140;i>=40;i--) {
+      FS.write (i);
+      delay (10);
+      s1.ping();    
+      moverover(i);
+    }
+  } else {
+    moverover(90);
+  }
+}
+
+void moverover(int deg){
+  if ( millimetres_s1 < 300 ) {
+    FSM=0;
+    if ( millimetres_s1 < 200 ) {
+      goback();
+      delay(700);
+      if ( deg >= 40 && deg <= 90 ) {
+        goleft (1);
+      } else if ( deg > 90 && deg <= 140 ) {
+        goright(1);
+      }
+    }
+    if (steer) {
+      if ( deg >= 40 && deg <= 90 ) {
+        if (DEBUG>=1) {
+          Serial.println ("obstacle ahead in under 400mm, goleft");
+        }
+        goleft (2);
+      } else if ( deg > 90 && deg <= 140 ) {
+        if (DEBUG>=1) {
+          Serial.println ("obstacle ahead in under 400mm, goright");
+        }          
+        goright (2);
+      }
+      steer = false;
+    }
+    forward = true; back = true;
+  } else {
+    FSM=1;
+    if (forward) {
+      if (DEBUG>=1) {
+        Serial.println ("no obstacle ahead, goforward");
+      }
+      goforward();
+      forward = false;
+    }
+    steer = true; back = true; 
   }
 }
 
@@ -122,24 +159,49 @@
   BR.write (BRC);
 }
 
-void goright () {
+void goright (int sp) {
   FL.write (FLC+steerspeed);
-  FR.write (FRC+steerspeed);
   BL.write (BLC+steerspeed);
-  BR.write (BRC-steerspeed);
+  if (sp==2) {
+    FR.write (FRC+steerspeed);
+    BR.write (BRC-steerspeed);
+  }
+}
+
+void goleft (int sp) {
+  if (sp==2) {
+    FL.write (FLC-steerspeed);
+    BL.write (BLC-steerspeed);
+  }
+  FR.write (FRC-steerspeed);  
+  BR.write (BRC+steerspeed);
 }
 
 void goforward () {
   FL.write (FLC+forwardspeed);
-  FR.write (FRC-forwardspeed);
   BL.write (BLC+forwardspeed);
+  FR.write (FRC-forwardspeed);  
   BR.write (BRC+forwardspeed);
 }
 
 void goback () {
   FL.write (FLC-backspeed);
+  BL.write (BLC-backspeed);  
   FR.write (FRC+backspeed);
-  BL.write (BLC-backspeed);
+  BR.write (BRC-backspeed);
+}
+
+void gobackleft () {
+  FL.write (FLC-backspeed);
+  BL.write (BLC-backspeed);  
+  FR.write (FRC-backspeed);
+  BR.write (BRC+backspeed);
+}
+
+void gobackright () {
+  FL.write (FLC+backspeed);
+  BL.write (BLC+backspeed);  
+  FR.write (FRC+backspeed);
   BR.write (BRC-backspeed);
 }
 
@@ -167,5 +229,7 @@
     }
   }
 }
+
+
 

 

comments