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

/** /**
* @author Razvan Stanga * @author Razvan Stanga
* *
**/ **/
   
#include <UltrasonicInterrupt.h> #include <UltrasonicInterrupt.h>
#include <Servo.h> #include <Servo.h>
   
int DEBUG=1; int DEBUG=1;
int ODEBUG=DEBUG; 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 MicrosecondsPerMillimetre = 1000.0 / SpeedOfSound; // microseconds per millimetre - sound travels 1 mm in ~2.9us
   
const float MicrosecondsToMillimetres = (1.0 / MicrosecondsPerMillimetre); const float MicrosecondsToMillimetres = (1.0 / MicrosecondsPerMillimetre);
const float MicrosecondsToMillimetres2 = MicrosecondsToMillimetres / 2.0; // beam travels the distance twice... so halve the time. const float MicrosecondsToMillimetres2 = MicrosecondsToMillimetres / 2.0; // beam travels the distance twice... so halve the time.
   
unsigned long lastTime = 0; unsigned long lastTime = 0;
   
int pingTimer = 0; int pingTimer = 0;
int pingDelay = 300; // milliseconds between ping pulses int pingDelay = 300; // milliseconds between ping pulses
int debugTimer = 0; int debugTimer = 0;
int debugDelay = 300; int debugDelay = 10;
  int servoTimer = 0;
  int servoDelay = 1000;
   
UltrasonicInterrupt s1; UltrasonicInterrupt s1;
const int SIGNAL1_PIN = 2; // digital pin connected to the trigger port on the module const int SIGNAL1_PIN = 2; // digital pin connected to the trigger port on the module
const int INTERRUPT1 = 2; // interrupt const int INTERRUPT1 = 2; // interrupt
const int INTERRUPT1_PIN = 0; // custom interrupt pin, see above const int INTERRUPT1_PIN = 0; // custom interrupt pin, see above
float millimetres_s1 = 0.0; float millimetres_s1 = 0.0;
   
Servo FL; int FLPIN = 3; int FLC = 86; Servo FL; int FLPIN = 3; int FLC = 86;
Servo FR; int FRPIN = 5; int FRC = 92; Servo FR; int FRPIN = 5; int FRC = 92;
Servo BL; int BLPIN = 9; int BLC = 93; Servo BL; int BLPIN = 9; int BLC = 93;
Servo BR; int BRPIN = 6; int BRC = 93; Servo BR; int BRPIN = 6; int BRC = 93;
  Servo FS; int FSPIN = 11; int FSC = 90; int FST = FSC; int FSM = 1;
int forwardspeed = 10;  
  int forwardspeed = 20;
int steerspeed = 10; int steerspeed = 10;
int backspeed = 5; int backspeed = 10;
bool forward = true; bool forward = true;
bool steer = true; bool steer = false;
bool back = true; bool back = false;
   
  int deg;
   
void setup() { void setup() {
   
if (DEBUG) { if (DEBUG) {
Serial.begin(9600); Serial.begin(9600);
} }
   
// start s1 sensor instance // start s1 sensor instance
s1.setup(INTERRUPT1, INTERRUPT1_PIN, SIGNAL1_PIN, callback_s1); s1.setup(INTERRUPT1, INTERRUPT1_PIN, SIGNAL1_PIN, callback_s1);
s1.begin(); s1.begin();
FL.attach(FLPIN); FL.attach(FLPIN);
FL.write (FLC); FL.write (FLC);
FR.attach(FRPIN); FR.attach(FRPIN);
FR.write (FRC); FR.write (FRC);
BL.attach(BLPIN); BL.attach(BLPIN);
BL.write (BLC); BL.write (BLC);
BR.attach(BRPIN); BR.attach(BRPIN);
BR.write (BRC); BR.write (BRC);
   
  FS.attach(FSPIN);
  FS.write (FSC);
   
  delay (3000);
  Serial.println ("------- start rover --------");
} }
   
void loop() { void loop() {
unsigned long time = millis(); unsigned long time = millis();
unsigned long dt = time - lastTime; unsigned long dt = time - lastTime;
lastTime = time; lastTime = time;
pingTimer += dt; pingTimer += dt;
debugTimer += dt; debugTimer += dt;
  servoTimer += dt;
if(pingTimer > pingDelay){ if(pingTimer > pingDelay){
pingTimer = 0; pingTimer = 0;
s1.ping(); 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){ if(debugTimer > debugDelay){
debugTimer = 0; debugTimer = 0;
DEBUG = ODEBUG; //DEBUG = ODEBUG;
} else { } else {
DEBUG = 0; //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;
} }
} }
   
void stop () { void stop () {
FL.write (FLC); FL.write (FLC);
FR.write (FRC); FR.write (FRC);
BL.write (BLC); BL.write (BLC);
BR.write (BRC); BR.write (BRC);
} }
   
void goright () { void goright (int sp) {
FL.write (FLC+steerspeed); FL.write (FLC+steerspeed);
FR.write (FRC+steerspeed);  
BL.write (BLC+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 () { void goforward () {
FL.write (FLC+forwardspeed); FL.write (FLC+forwardspeed);
FR.write (FRC-forwardspeed);  
BL.write (BLC+forwardspeed); BL.write (BLC+forwardspeed);
  FR.write (FRC-forwardspeed);
BR.write (BRC+forwardspeed); BR.write (BRC+forwardspeed);
} }
   
void goback () { void goback () {
FL.write (FLC-backspeed); FL.write (FLC-backspeed);
  BL.write (BLC-backspeed);
FR.write (FRC+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); BR.write (BRC-backspeed);
} }
   
/** /**
* Pulse complete callback handler for UltrasonicInterrupt s1 instance * Pulse complete callback handler for UltrasonicInterrupt s1 instance
* @param duration - pulse length in microseconds * @param duration - pulse length in microseconds
*/ */
void callback_s1(unsigned long duration) { void callback_s1(unsigned long duration) {
   
millimetres_s1 = MicrosecondsToMillimetres2 * duration; millimetres_s1 = MicrosecondsToMillimetres2 * duration;
   
if(millimetres_s1 > 4000){ if(millimetres_s1 > 4000){
if (DEBUG>=2){ if (DEBUG>=2){
Serial.println("s1 out of range"); Serial.println("s1 out of range");
} }
// out of range (http://users.ece.utexas.edu/~valvano/Datasheets/HCSR04b.pdf) // out of range (http://users.ece.utexas.edu/~valvano/Datasheets/HCSR04b.pdf)
millimetres_s1 = 4000; millimetres_s1 = 4000;
} else { } else {
if (DEBUG>=2){ if (DEBUG>=2){
Serial.print("s1 duration: "); Serial.print("s1 duration: ");
Serial.print(duration); Serial.print(duration);
Serial.print(" us, distance: "); Serial.print(" us, distance: ");
Serial.print(millimetres_s1); Serial.print(millimetres_s1);
Serial.println(" mm"); Serial.println(" mm");
} }
} }
} }
   
   
   
comments