Let's Make Robots!

servo and ping

anyone have some code i can have a look at to intergrate the ping sensor: ie have it look both ways and decide what way to turn?. i have been trying to find somethign similar but all i have seen is for the picaxe, i have an arduino.

thanks

 

Comment viewing options

Select your preferred way to display the comments and click "Save settings" to activate your changes.

wow cool, i have an idea, i think i can make it work with adafruit motor shield. :P

 


int motorR_1 = 5; //Right motor ENABLE pin
int motorR_2 = 4; //Right motor PWN (SPEED) pin
int motorL_1 = 3; //Right motor ENABLE pin
int motorL_2 = 2; //Right motor PWN (SPEED) pin

int servoPin1 = 1; // Servo connect to arduino pin 1
int left = 600; // Pulse width of 600µs to turn left
int right = 2400; // Pulse width of 2400µs to turn right
int straight = 1500; // Pulse width of 1500µs to return straight
int pulse = 0; // Pulse to send to servo

long lastPulse = 0; // the time in milliseconds of the last pulse
int refreshTime = 20; // the time needed between pulses

int pingPin = 7; // ultrasonic sensor pin
int leftDist = 0; // distance to left obstacle
int rightDist = 0; // distance to right obstacle

void setup()
{
  pinMode(motorR_1, OUTPUT); // declare motor pin as OUTPUT
  pinMode(motorR_2, OUTPUT); // declare motor pin as OUTPUT
  pinMode(motorL_1, OUTPUT); // declare motor pin as OUTPUT
  pinMode(motorL_2, OUTPUT); // declare motor pin as OUTPUT
  pinMode(servoPin1, OUTPUT); // declare servo pin as OUTPUT
  pulse = straight; // face the servo forward
}

void loop()
{
  long duration, cm;
 
  digitalWrite (motorR_1, HIGH); // Move Right motor FORWARD...
  digitalWrite (motorR_2, HIGH); // ...at full speed.
  digitalWrite (motorL_1, HIGH); // Move Left motor FORWARD...
  digitalWrite (motorL_2, HIGH); // ...at full speed.
 
  pulse = straight; // face the sensor forward
  pinMode(pingPin, OUTPUT);
  digitalWrite(pingPin, LOW); // refreash the sensor
  delayMicroseconds(2);
  digitalWrite(pingPin, HIGH); // send ultrasonic pulse
  delayMicroseconds(5);
  digitalWrite(pingPin, LOW); // turn sensor off
  pinMode(pingPin, INPUT); // allow for sensor to recieve echo
 
  duration = pulseIn(pingPin, HIGH); // wait for sensor to reieve echo
  cm = microsecondsToCentimeters(duration); // record distance to cm
 
  if (cm < 15) // if the recorded distance is less than 15cm
  {
    pinMode(motorL_2, INPUT); // brake left motor
    pinMode(motorR_2, INPUT); // brake right motor
    delay(500); // wait for the motors to stop completely
    digitalWrite(motorR_1, LOW); // disable right motor    
    digitalWrite(motorL_1, LOW); // diable left motor
    
    
    digitalWrite(pingPin, LOW); // stop the sensor
    pulse = left; // turn the sensor left
    delay(100); // wait for the servo to turn to the left
    digitalWrite(pingPin, HIGH); // send ultrasonic pulse
    delayMicroseconds(5);
    digitalWrite(pingPin, LOW); // turn sensor off
    pinMode(pingPin, INPUT); // allow for sensor to recieve echo
    
    duration = pulseIn(pingPin, HIGH); // wait for sensor to reieve echo
    leftDist = microsecondsToCentimeters(duration); // record distance to leftDist
    
    digitalWrite(pingPin, LOW); // stop the sensor
    pulse = right; // turn the sensor right
    delay(100); // wait for servo to turn to the right
    digitalWrite(pingPin, HIGH); // send ultrasonic pulse
    delayMicroseconds(5);
    digitalWrite(pingPin, LOW); // turn sensor off
    pinMode(pingPin, INPUT); // allow for sensor to recieve echo
    
    duration = pulseIn(pingPin, HIGH); // wait for sensor to recieve echo
    rightDist = microsecondsToCentimeters(duration); // record distance to rightDist
  }
 
  if (leftDist > rightDist)
  {
    digitalWrite(motorR_1, HIGH); // turn right motor power on
    pinMode(motorR_2, OUTPUT); // release right motor
    digitalWrite(motorL_1, HIGH); // turn left motor power on
    pinMode(motorL_2, OUTPUT); // release left motor
    
    digitalWrite(motorR_2, HIGH);
    digitalWrite(motorL_2, LOW);
    delay(1000);
  }    
 
   if (rightDist > leftDist)
  {
    digitalWrite(motorR_1, HIGH); // turn right motor power on
    pinMode(motorR_2, OUTPUT); // release right motor
    digitalWrite(motorL_1, HIGH); // turn left motor power on
    pinMode(motorL_2, OUTPUT); // release left motor
    
    digitalWrite(motorR_2, LOW);
    digitalWrite(motorL_2, HIGH);
    delay(1000);
  }  

    
  if (millis() - lastPulse >= refreshTime)
  {
    digitalWrite(servoPin1, HIGH); // turn servo on
    delayMicroseconds(pulse); // length of the pulse sets the motor position
    digitalWrite(servoPin1, LOW); // turn servo off
    lastPulse = millis();
  }  
}

long microsecondsToCentimeters(long microseconds)
{
  return microseconds / 29 / 2;
}

/*
Using the motor controller (SN754410 or equivalent and 2 tri-state switches)

Pin 1 - is used to Enable or Disable the motor controller chip. Whenever the controller is disabled then it is in 'coast' mode. So if we ask the motor to go in a particular clockwise direction then we can use this pin to set the speed that it turns. If it aways high then the motor rotates at full speed. If it is always low then the motor is disconnected and so doesn't turn. By using PWM we can control the speed of the motor from 100% to 0% duty cycle. So pin 1 is the 'throttle' or 'accelerator'.

Pin 2 - Sets the direction of the motor - Forward or Reverse. If this input is high then Input 1 is high, which turns on the transistor so Input 2 is low. This makes the motor turn one way. If this input is low then Input 2 is low, the transistor is off and so input 2 is high, and the motor turns the other way. But the cool thing is that if this pin is disconnected then Input 1 and Input 2 are both in the same state which means that the motor will brake. How do you 'disconnect' a wire that is soldered in - all we do is change the micro-processor pin to be an input pin and the built in resistors make this wire 'disconnected'
*/

 

 


You could use a circuit like this:

 

anlh5v.jpg

For information about tri-state switches and the sn754410 or equivalent look here: http://www.societyofrobots.com/member_tutorials/node/161

I hope the code is somewhat what your looking for or can at least help steer you in the right direction. I tried to comment it as much as possible, but if you have any questions just ask.