HC-SR04 / servo code help for AmsterBot :)

Community General Discussions
,
1

420498_412051402145496_642280519_n.jpg (165663Bytes)

 

 

I have a new video and pics that I will post soon of Amsterbot's new/temp body. 

My questions on the code are simple I guess. I have gotten the HC-sr04 to work with example code and I can make use the servo code with ease, even playing around with it in lots of fun ways. My brick wall for now is how to put that together? I would like to start with simple code that triggers the head servo to rotate when something is 6 cm away? or something like this. After that I plan to build on this and in the end have a complete amsterbot. Many thanks in advance :D

I'm also using Duemilanove w/atmega 328p just in case.

If I knew where to start I would not be asking. I have been trying to figure this out myself by looking at example code and such. I do not understand how to tie together the two components through code. I am not asking someone to write the code for my whole robot I am just looking for help to break through this understanding issue. I have put off asking for help with this for months, I also run a business and have a life outside of robots so it's not something I get to work on everyday. The bad part about that is I end up with some other issue like a bad cable that takes me weeks to figure out. It was new lol. 

Here is the code I am working with/on.. not much to look at.

 

 

/*

  HC-SR04 Ping distance sensor]

  VCC to arduino 5v GND to arduino GND

  Echo to Arduino pin 13 Trig to Arduino pin 12

*/

 

int echoPin = 13; 

int trigPin = 12; 

 

void setup() { 

  Serial.begin (9600); 

  pinMode(trigPin, OUTPUT); 

  pinMode(echoPin, INPUT); 

} 

 

void loop() { 

  int duration, cm; 

  digitalWrite(trigPin, LOW); 

  delayMicroseconds(2); 

  digitalWrite(trigPin, HIGH); 

  delayMicroseconds(10); 

  digitalWrite(trigPin, LOW); 

  duration = pulseIn(echoPin, HIGH); 

  cm = duration / 29 / 2;

 /* Sound velocity=29cm/microsec,

    then divide by 2 because of the return time */

  Serial.print(cm); 

  Serial.println(" cm"); 

  delay(500); 

}

 

 

Next bit of code is the servo code I am playing with for amsterbot. Hey he moves and does what I want so don't be to hard on me here. I also added some led blink action. 

 

 #include <Servo.h> 

           // amsterbot 2.0 3/1/12 new 4/18/12 joe chiefdadddy nyc

Servo servoLeft;           // Define left servo

Servo servoRight;          // Define right servo

Servo servohead;           // head           

void setup() 

{ 

  servoLeft.attach(10);  // Set left servo to digital pin 10

  servoRight.attach(9);  // Set right servo to digital pin 9

  servohead.attach(6);   // head servo

  servoxtra.attach(11);  // xtra led servo

  pinMode(13, OUTPUT);   // leds

}

 

 

 

void loop()              // Loop through motion tests

{ 

  stopRobot();

  robotlookforward();

  delay(500);

  robotlookleft();

  delay(500);

  robotlookforward();

  delay(500);

  robotlookright();

  delay(500);

  robotlookforward();

  delay(500);

  startRobot();

  turnRight();

  delay(600);

  forward();             

  delay(2000);            

}

 

                           // Motion routines for forward, reverse, turns, and stop head servo and xtraservo

void forward()     

{

  servoLeft.write(0);

  servoRight.write(180);

  servohead.write(90);

  servoxtra.write(180);

  digitalWrite(13, HIGH);   

  delay(250);              

  digitalWrite(13, LOW);    

  delay(250);              

  digitalWrite(13, HIGH);   

  delay(100);             

  digitalWrite(13, LOW);    

  delay(100);             

  digitalWrite(13, HIGH);  

  delay(100);              

  digitalWrite(13, LOW);  

  delay(250);             

  digitalWrite(13, HIGH);  

}

 

 

void reverse()

{

  servoLeft.write(180);

  servoRight.write(0);

  servohead.write(0);

  servoxtra.write(90);

  digitalWrite(13, HIGH);   

  delay(250);              

  digitalWrite(13, LOW);    

  delay(250);              

  digitalWrite(13, HIGH);   

  delay(100);             

  digitalWrite(13, LOW);   

  delay(500);             

  digitalWrite(13, HIGH);   

  delay(500);            

  digitalWrite(13, LOW);  

  delay(250);              

  digitalWrite(13, HIGH);  

  delay(250);              

  digitalWrite(13, LOW);    

  delay(250);              

  digitalWrite(13, HIGH);   

  delay(100);             

  digitalWrite(13, LOW);    

  delay(100);              

  digitalWrite(13, HIGH);   

  delay(100);              

  digitalWrite(13, LOW);    

  delay(250);              

}

 

void turnRight()

{

  servoLeft.write(180);

  servoRight.write(180);

  servoxtra.write(0);

  digitalWrite(13, LOW);   

}

 

void turnLeft()

{

  servoLeft.write(0);

  servoRight.write(0);

  servoxtra.write(180);

  digitalWrite(13, LOW);   

}

 

void stopRobot()

{

  servoLeft.detach();

  servoRight.detach();

  servohead.write(90);

  servoxtra.write(90);

  digitalWrite(13, LOW);   

  digitalWrite(13, HIGH);   

  delay(250);              

  digitalWrite(13, LOW);           

}

void startRobot()

{

  servoLeft.attach(10);

  servoRight.attach(9);

  servohead.write(90);

  digitalWrite(13, HIGH);   //set led on

}

void robotlookforward()

{

  servohead.write(90);

  digitalWrite(13, HIGH);   // set the LED on

 

}

void robotlookleft()

{

  servohead.write(180);

  digitalWrite(13, LOW);    // set LED off

}

void robotlookright()

{

  servohead.write(0);

  digitalWrite(13, HIGH);   // set the LED on

 

}

 

 

All I am looking for here is a simple way to make the head servo move when the HC-sr04 is triggered at a certain distance. I can continue to build my code skills from there. Like I said I would like to keep this simple for now.

and for your viewing pleasure Chiefdaddy's Beard and Chief the wonder dog! :D

2

The basic idea

is take a reading from the sensor and test the distance. If the distance is too short, then scan for a way out. I am going to show a bit of almost psuedocode.

distanceRead = scan();
if (distanceRead < dangerDistance) {
    avoid();
}

void avoid() {
    lookRight(); //function to turn servo to the right
    distanceRight = scan();
    lookLeft(); //function to turn servo to the left
    distanceLeft = scan();
    if (distanceLeft < distanceRight) {
         turnRight();
    }
    else {
        turnLeft();
    }
} 

 

3

hmm

 

I still have two pieces of code here the sensor and servo.

do I include this at the top?

 #include <Servo.h> 

Then start with… 

int echoPin = 13; 

int trigPin = 12; 

 

void setup() { 

  Serial.begin (9600); 

  pinMode(trigPin, OUTPUT); 

  pinMode(echoPin, INPUT); 

} 

4

I will hope this works for you.

I have not really proof read this. There may be errors in it.

#include <Servo.h>

 

// amsterbot 2.0 3/1/12 new 4/18/12 joe chiefdadddy nyc

 

/

HC-SR04 Ping distance sensor]

VCC to arduino 5v GND to arduino GND

Echo to Arduino pin 13 Trig to Arduino pin 12

/

 

Servo servoLeft; // Define left servo

Servo servoRight; // Define right servo

Servo servohead; // head

int echoPin = 13;

int trigPin = 12;

 

void setup() {

Serial.begin (9600);

pinMode(trigPin, OUTPUT);

pinMode(echoPin, INPUT);

servoLeft.attach(10); // Set left servo to digital pin 10

servoRight.attach(9); // Set right servo to digital pin 9

servohead.attach(6); // head servo

servoxtra.attach(11); // xtra led servo

pinMode(13, OUTPUT); // leds

}

 

void loop() {

int distanceRead;

 

stopRobot();

distanceRead = scan();

startRobot();

if (distanceRead < dangerDistance) {

avoid();

}

else {

forward();

}

}

 

void avoid() {

robotLookRight(); //function to turn servo to the right

distanceRight = scan();

robotLookLeft(); //function to turn servo to the left

distanceLeft = scan();

 

if (distanceLeft < distanceRight) {

turnRight();

}

else {

turnLeft();

}

 

robotLookForward();

}

 

int scan() {

int duration, cm;

 

digitalWrite(trigPin, LOW);

delayMicroseconds(2);

digitalWrite(trigPin, HIGH);

delayMicroseconds(10);

digitalWrite(trigPin, LOW);

 

duration = pulseIn(echoPin, HIGH);

cm = duration / 29 / 2;

 

/* Sound velocity=29cm/microsec,

then divide by 2 because of the return time */

//Serial.print(cm);

//Serial.println(" cm");

//delay(500);

 

return cm;

}

 

void forward()

{

servoLeft.write(0);

servoRight.write(180);

servohead.write(90);

servoxtra.write(180);

digitalWrite(13, HIGH);

delay(250);

digitalWrite(13, LOW);

delay(250);

digitalWrite(13, HIGH);

delay(100);

digitalWrite(13, LOW);

delay(100);

digitalWrite(13, HIGH);

delay(100);

digitalWrite(13, LOW);

delay(250);

digitalWrite(13, HIGH);

}

 

void reverse()

{

servoLeft.write(180);

servoRight.write(0);

servohead.write(0);

servoxtra.write(90);

digitalWrite(13, HIGH);

delay(250);

digitalWrite(13, LOW);

delay(250);

digitalWrite(13, HIGH);

delay(100);

digitalWrite(13, LOW);

delay(500);

digitalWrite(13, HIGH);

delay(500);

digitalWrite(13, LOW);

delay(250);

digitalWrite(13, HIGH);

delay(250);

digitalWrite(13, LOW);

delay(250);

digitalWrite(13, HIGH);

delay(100);

digitalWrite(13, LOW);

delay(100);

digitalWrite(13, HIGH);

delay(100);

digitalWrite(13, LOW);

delay(250);

}

 

void turnRight()

{

servoLeft.write(180);

servoRight.write(180);

servoxtra.write(0);

digitalWrite(13, LOW);

}

 

void turnLeft()

{

servoLeft.write(0);

servoRight.write(0);

servoxtra.write(180);

digitalWrite(13, LOW);

}

 

void stopRobot()

{

servoLeft.detach();

servoRight.detach();

servohead.write(90);

servoxtra.write(90);

digitalWrite(13, LOW);

digitalWrite(13, HIGH);

delay(250);

digitalWrite(13, LOW);

}

 

void startRobot()

{

servoLeft.attach(10);

servoRight.attach(9);

servohead.write(90);

digitalWrite(13, HIGH); //set led on

}

 

void robotLookForward()

{

servohead.write(90);

digitalWrite(13, HIGH); // set the LED on

}

 

void robotLookLeft()

{

servohead.write(180);

digitalWrite(13, LOW); // set LED off

}

 

void robotLookRight()

{

servohead.write(0);

digitalWrite(13, HIGH); // set the LED on

}

5

Need to define

You need to define dangerDistance to the max safe distance to obstacle.

6

I’m new to Arduino coding

I’m new to Arduino coding so probably not the best source for help but I’m also using the HC-SR04. I’m using the ultrasonic.h library in my code.

Ultrasonic Distance Sensor
Trigger - 6, Echo - 7
Servo - 5
/

/-----( Import needed libraries )-----/
#include <Ultrasonic.h>
#include <Servo.h>

/-----( Declare Constants and Pin Numbers )-----/
#define  TrigPin 6
#define  EchoPin 7

/-----( Declare objects )-----/
Ultrasonic PingModule(TrigPin, EchoPin);
Servo PingServo;     // create servo object to control a servo

I have min safe distance at 16 inches right now.

/-----( Declare Variables )-----/
int minSafeDist = 16                 // Minimum distance for ping sensor to know when to turn
int centerDist, leftDist, rightDist; // Define variables center, left and right distance

My servo is on pin 5

void setup(){
  Serial.begin(9600);
  PingServo.attach(5);                   // attaches pin 5 to the servo object

Then the looping code:

/***** LOOP: RUNS CONSTANTLY ******/
void loop(){
  LookAhead();
  Serial.print("Distance = ");
  Serial.println(centerDist);
  if (centerDist >= minSafeDist)         // no obstacle detected
    {
    Forward();                                    // Move forward
  }else{                                             // obstacle detected ahead
    LookAround();                               // Check your surroundings for best route
    if (leftDist&&rightDist<minSafeDist) // If the left and right distance is smaller than the min safe distance (16 inch) go back
    {
    BumpSwitch();                              // backup and turn
    }else{
    GetDirection();
    }
  }
  delay(100);
 }
//–(end main loop )—

Looking around functions are like this:

void LookAhead(){
  Serial.println(“LookAhead”);
  PingServo.write(90);                   // angle to look forward
  delay(150);                               // wait 0.15 seconds
  centerDist = PingModule.Ranging(INC);  //get the center distance
  return;
}

void LookAround(){
  Serial.println(“LookAround”);
  PingServo.write(125);                  // 125° angle (look right)
  delay(150);                                // wait 0.15 seconds
  rightDist = PingModule.Ranging(INC);   //get the right distance
  PingServo.write(55);                   // 55° angle (look left)
  delay(150);                               // wait 0.15 seconds
  leftDist = PingModule.Ranging(INC);    // get the left distance
  PingServo.write(90);                   // 90° angle (look forward)
  delay(100);                               // wait 0.10 seconds
  return;
}

void GetDirection(){
  if( rightDist > leftDist)          // If the right distance is greater than the left distance , turn right
  {
  turnRight();
  }else if (leftDist > rightDist)  // If the left distance is greater than the right distance , turn left
  {
  turnLeft();
  }
  return;
}

void BumpSwitch(){
  Serial.println(“BumpSwitch”);
  Stop();
  Reverse();
  LookAround();
  GetDirection();
}

I’m still playing with the values for left/right look angles, min distance and delay values.

Just some more options, don’t mean to confuse you.