Simple test of ultrasonic sensor HC-SR04 connected to a Dagu mini driver (arduino compatible board) and a two-wheel Dagu chassis.

dagu_ultrasonic

//----------------------------------------------------------
// dagu_ultrasonic.ino
//
// Simple test of ultrasonic sensor HC-SR04 connected to
// Dagu mini driver (arduino compatible board) and a two 
// wheel Dagu chassis.
//
// Author: Mahesh Venkitachalam
// Website: electronut.in
//----------------------------------------------------------
 
int pinTrigger = 2;
int pinEcho = 4;
int pinLMDir = 7;
int pinLMPWM = 9;
int pinRMDir = 8;
int pinRMPWM = 10;

//----------------------------------------------------------
// send a ping from ultrasonic sensor HC-SR04 and return 
// distance in cm
float ping()
{
  // send a 10us+ pulse
  digitalWrite(pinTrigger, LOW);
  delayMicroseconds(20);
  digitalWrite(pinTrigger, HIGH);
  delayMicroseconds(10);
  digitalWrite(pinTrigger, LOW);
  delayMicroseconds(20);
  
  //  read duration of echo 
  int duration = pulseIn(pinEcho, HIGH);

  // dist = duration * speed of sound * 1/2
  // dist in cm = duration in us * 1 x 10^{-6} * 340.26 * 100 * 1/2
  // =  0.017*duration
  float dist = 0.017 * duration;
  
  return dist;
}

//----------------------------------------------------------
void setup()
{
    // set up motor pins
    pinMode(pinLMDir, OUTPUT );
    pinMode(pinLMPWM, OUTPUT );
    pinMode(pinRMDir, OUTPUT );
    pinMode(pinRMPWM, OUTPUT );
    
     // set up sensor pins 
    pinMode(pinTrigger, OUTPUT);
    pinMode(pinEcho, INPUT);
}

//----------------------------------------------------------
// move forward/bacwa
void move(bool forward, int timeMS, int speedPercent)
{
  // set speed
  int speedPWM = (255*speedPercent)/100;
  
  // set direction
  if(forward) {
      digitalWrite(pinLMDir, HIGH);
      digitalWrite(pinRMDir, LOW);
  }
  else {
      digitalWrite(pinLMDir, LOW);
      digitalWrite(pinRMDir, HIGH);
  }
  
  // set speed
  analogWrite(pinLMPWM, speedPWM);
  analogWrite(pinRMPWM, speedPWM);
  
  // sleep so it moves
  delay(timeMS);
}

//----------------------------------------------------------
// stop moving
void stop()
{
    analogWrite(pinLMPWM, 0);
    analogWrite(pinRMPWM, 0);
}

//----------------------------------------------------------
// turn 
void turn(bool left, int turnMS)
{
    // set direction
    if(left) {
       digitalWrite(pinLMDir, LOW);
       digitalWrite(pinRMDir, LOW);
    }
    else {
       digitalWrite(pinLMDir, HIGH);
       digitalWrite(pinRMDir, HIGH);      
    }
    // turn at 25 % speed
    analogWrite(pinLMPWM, 64);
    analogWrite(pinRMPWM, 64);
    // for x ms
    delay(turnMS);
    // stop
    analogWrite(pinLMPWM, 0);
    analogWrite(pinRMPWM, 0);
        
    // reset direction
    digitalWrite(pinLMDir, HIGH);
    digitalWrite(pinRMDir, LOW);
}
 
//----------------------------------------------------------
void loop()
{ 

  // send a ping
  float dist = ping();
   
  // is there an obstruction?
  if (dist > 0 && dist < 25) {  
    // stop 
    stop();  
    // move back
    move(false, 250, 60);
    // turn left
    turn(true, 100);
  }
  else 
  {
    move(true, 500, 40);
  }    
    
  delay(500);   
}