initial commit
initial commit

file:b/rover/rover.ino (new)
  /**
  * @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