Go Down

Topic: Obstacle Avoiding Robot (Read 3479 times) previous topic - next topic

chrisclifford

Hi all,

2 friends and I are making a robot using our Arduino Leonardo which will move forwards and avoid obstacles in its path. We are using components from Vex Robotics, which you may or may not be familiar with. We are currently using 3 Vex motors, 3 wheels attached to said motors, and two Vex ultrasonic sensors. We have setup the bot although we are running into problems while trying to code it. We were wondering if any Arduino vets could supply us with some sample code for this project, or perhaps supply code they have used on a similar project before. Currently our ultrasonic sensors are returning values of 1 and 2 randomly and returns no other values. I will attach our code so you can give us advice on it or possibly supply new code. So far we greatly enjoy the Arduino and hopefully this community can help us shed some light on our problem. Thank you,

Chris

Code: [Select]
#include "Servo.h"
#include <SoftwareSerial.h>

const int Trig_pinRight =  12;   // Triggers Pulse for RIGHT Ultrasonic Sensor
const int Echo_pinRight = 13;     // Recevies Echo for RIGHT Ultrasonic Sensor
const int Trig_pinLeft = 4;   // Triggers Pulse for LEFT Ultrasonic Sensor
const int Echo_pinLeft = 5;     // Receives Echo for LEFT Ultrasonic Sensor
long durationRight;            // Time it takes for pulse to bounce back to RIGHT Ultrasonic Sensor
long durationLeft;             // Time it takes for pulse to bounce back to LEFT Ultrasonic Sensor

Servo motorLeft;               // LEFT VEX Motor
Servo motorRight;              // RIGHT VEX Motor

void setup() {

  Serial.begin(9600);           // Set up Serial library at 9600 bps

  Serial.println("Initializing...");  // Print Initializing... to confirm code is working with Serial Library

  motorLeft.attach(9);         // LEFT Motor is in pin 9
  motorRight.attach(11);        // RIGHT Motor is in pin 11


  Serial.println ("Starting");
  // initialize the pulse pin as output:
  pinMode(Trig_pinRight, OUTPUT);     
  // initialize the pulse pin as output:
  pinMode(Trig_pinLeft, OUTPUT);     
  // initialize the echo_pin pin as an input:
  pinMode(Echo_pinRight, INPUT);
  // initialize the echo_pin pin as an input:
  pinMode(Echo_pinLeft, INPUT);     

}


void loop()
  {                                                         
   
    // Pulse and Echo for RIGHT Ultrasonic Sensor ; Process for determining and printing the durations for RIGHT Ultrasonic Sensor
   
    digitalWrite(Trig_pinRight, LOW);   
    delayMicroseconds(2);
    digitalWrite(Trig_pinRight, HIGH);
    delayMicroseconds(5);
    digitalWrite(Trig_pinRight, LOW);
    durationRight = pulseIn(Echo_pinRight,HIGH);
    Serial.println("durationRight: ");
    Serial.println(durationRight, DEC);
   
    // Pulse and Echo for LEFT Ultrasonic Sensor ; Process for determining and printing the durations for LEFT Ultrasonic Sensor
   
    digitalWrite(Trig_pinLeft, LOW);
    delayMicroseconds(2);
    digitalWrite(Trig_pinLeft, HIGH);
    delayMicroseconds(5);
    digitalWrite(Trig_pinLeft, LOW);
    durationLeft = pulseIn(Echo_pinLeft,HIGH);
    Serial.println("durationLeft: ");
    Serial.println(durationLeft, DEC);
   
    if(durationRight > 0 && durationRight < 4000 && durationRight > 0 && durationLeft < 4000) // STOP when HUMAN is too close to both sensors
        {
            motorLeft.write(90);
            motorRight.write(90);
        }
    if(durationRight > 4000 && durationRight < 10000 && durationLeft > 4000 && durationLeft < 10000) // MOVE FORWARD when HUMAN is equadistant to both sensors
        {
            motorLeft.write(113);
            motorRight.write(68);
        }
    if(durationLeft - 1000 > durationRight && durationRight > 4000) // TURN RIGHT when HUMAN is close to RIGHT sensor
        {
            motorLeft.write(120);
            motorRight.write(100);
            delay(500);
           
            motorLeft.write(120);
            motorRight.write(75);
            delay(2000);
        }
    if(durationRight - 1000 > durationLeft && durationLeft > 4000) // TURN LEFT when HUMAN is closer to LEFT sensor
        {
            motorLeft.write(100);
            motorRight.write(75);
            delay(500);
           
            motorLeft.write(120);
            motorRight.write(75);
            delay(5000);
        }
    if(durationRight > 10000 && durationLeft > 10000) // MOVE FORWARD FAST when HUMAN is too far away but still equadistant
        {
            motorLeft.write(135);
            motorRight.write(45);
        }
  }

gelfling6

Code looks about right, But adding the SoftwareSerial library might be overkill, as your writing the return data out to the standard Serial I/O. (pins D0 & D1, which are connected to the USB-to-Serial.)   You can also wire a bluetooth or XBEE radio to those same pins for Serial I/O, but they will ignore the USB serial.

I'm also trying to make a tank rover using the VEX as a drive platform, wiring everything through the Arduino, too. (occupying D11 to 2, as LeftMotor, RightMotor, SonarServo, PingIn, PingOut, LeftLine, MidLine, RightLine, SoftIn, SoftOut, using 2 VEX motor drives,
a single VEX Servo as a front-only 180-degree sense, the VEX Sonic Range Finder, 3 line sensors (spaced apart by 1.5") and RX & TX to the 9600 XBEE Series-1.  I see you're using raw distances, which might give some odd readings. the code I've written for mine, (still a work in progress) uses the conversion from raw to cm distance (actually stripped out the Inches conversion from the example code.)

I'm trying to get mine to run in 1 of 3 modes.. Mode 1, follow key presses from the laptop (straight Parallax USB to XBEE Explorer), to the Adafruit XBEE adapter. (pins you would normally connect the FTDI USB cable.).. Mode 2, random explore, stopping 10CM from anything that pops up in front of it, searching 0-179 in 5 steps, looking for the farthest object, turning and going that direction, Or mode 3, watching the line follower sensors for a 3" wide white line on a running track, and staying centered. (if either of the outer sensors see a change, turn towards it to correct path. If it sees a change on the center, stop.)

I'm trying to see if I can change the line sensors to analog pins, and re-assign two digital pins to a pair of step counters (the original version, not the offset. ).. but I'll need to re-work the frame to accommodate the rotary encoders.  I still have the code at home, on a Linux laptop..

TomGeorge

#2
Jun 01, 2014, 09:21 am Last Edit: Jun 01, 2014, 09:39 am by TomGeorge Reason: 1
Hi, can I suggest you start with one sensor and get it working before adding the second and the motors.
The key is to get your sketch done in parts, each part a different input or output.
So I'd suggest start anew and just write code for one sensor and debug it, that way you know that if a proble exists it is in the code you are working with, not some other part like the motor or other sensor.
Just a suggestion, makes debugging soooo much less confusing.
try.
http://forum.arduino.cc/index.php/topic,94499.0.html
http://www.robotc.net/wiki/Tutorials/Arduino_Projects/Mobile_Robotics/VEX/Use_Ultrasonic_Sensor_To_Measure_Distance


Tom...... :)

ps, bump your serial speed up to 115200.
pps, instructibles programs have been found to be sometimes unreliable too and familiar.
http://www.instructables.com/id/The-Lazy-American-Robot/?ALLSTEPS
Everything runs on smoke, let the smoke out, it stops running....

Go Up