Let's Make Robots!

who wants to help with code?

this is the code for my new robot it uses two servos to drive forward and a ping mounted on a servo that sweeps back and forth when it senses something 9inches or closer it backs up and turns right. what i would like it to do is when is sees something it will take the angle of the servo and if its right it will turn left if its left it will turn right. any ideas? btw i cant claim the two main peices of code one was found here will give credit when i remember who posted it. 2nd is the ping sample

 

 

 

 

 

 


 

//servo bot with ping sweeper

#include <Servo.h>

byte currentPos = 0;
byte positioner = 15;
const int pingPin = 7; //ping on pin 7
Servo leftservo;
Servo rightservo;
Servo pingservo;

void moveServo()
{
if ( currentPos == 0 )
{
positioner = 20;
}
else if ( currentPos == 180 )
{
positioner = -20;
}
currentPos += positioner;
pingservo.write(currentPos);
}
 
 
void setup()
{
  leftservo.attach(9);  //left servo pin 9
  rightservo.attach(10); //right servo pin 10
  pingservo.attach(11); //ping servo pin 11
  Serial.begin(9600);
}
 
void loop()
{
 
  // establish variables for duration of the ping,
  // and the distance result in inches and centimeters:
  long duration, inches, cm;

  // The PING))) is triggered by a HIGH pulse of 2 or more microseconds.
  // Give a short LOW pulse beforehand to ensure a clean HIGH pulse:
  pinMode(pingPin, OUTPUT);
  digitalWrite(pingPin, LOW);
  delayMicroseconds(2);
  digitalWrite(pingPin, HIGH);
  delayMicroseconds(5);
  digitalWrite(pingPin, LOW);

  // The same pin is used to read the signal from the PING))): a HIGH
  // pulse whose duration is the time (in microseconds) from the sending
  // of the ping to the reception of its echo off of an object.
  pinMode(pingPin, INPUT);
  duration = pulseIn(pingPin, HIGH);

  // convert the time into a distance
  inches = microsecondsToInches(duration);
  cm = microsecondsToCentimeters(duration);
 
  Serial.print(inches);
  Serial.print("in, ");
  Serial.print(cm);
  Serial.print("cm");
  Serial.println();
 
 
delay(150);
if( inches <= 9 )//if the robot sees something 9in or closer it backs up and turns
{
backward();
delay(500);
right();
delay(900);

}
else if (inches >9 )//if not it goes forward
{
forward();
delay(100);
hault();
}
moveServo();

}

long microsecondsToInches(long microseconds)
{
  // According to Parallax's datasheet for the PING))), there are
  // 73.746 microseconds per inch (i.e. sound travels at 1130 feet per
  // second).  This gives the distance travelled by the ping, outbound
  // and return, so we divide by 2 to get the distance of the obstacle.
  // See: http://www.parallax.com/dl/docs/prod/acc/28015-PING-v1.3.pdf
  return microseconds / 74 / 2;
}

long microsecondsToCentimeters(long microseconds)
{
  // The speed of sound is 340 m/s or 29 microseconds per centimeter.
  // The ping travels out and back, so to find the distance of the
  // object we take half of the distance travelled.
  return microseconds / 29 / 2;
}

void forward()
{
 leftservo.write(0);
 rightservo.write(180);
}

void backward()
{
 leftservo.write(180);
 rightservo.write(0);
}

void left()
{
 leftservo.write(180);
 rightservo.write(180);
}

void right()
{
 leftservo.write(0);
 rightservo.write(0);
}

void hault()
{
 leftservo.write(90);
 rightservo.write(90);
}

Comment viewing options

Select your preferred way to display the comments and click "Save settings" to activate your changes.
thank youmuch its exactly what i was trying to do works perfect

Here's a slight modification to part of your main loop that should do the trick (you may have to swap the left() and right() commands around depending on your sensor servo's orientation): 

if( inches <= 9 )                   //if the robot sees something 9in or closer it backs up and turns
{
backward();                         //Move backwards for 500 delay
delay(500);
     if( currentPos < 90 )        //If the sensor is pointing to the left...
     {
     right();                          //Turn right for 900 delay
     }
     else if( currentPos > 90 ) //If the sensor is pointing to the right...
     {
     left();                           //Turn left for 900 delay
     }
delay(900);
}