Go Down

Topic: problems with robot using ultrasonic sensor to avoid obstacles [solved] (Read 2865 times) previous topic - next topic

jorpec

Hi

I think it would more simple if you create functions to move forward, backwads, turn left/right

At a moment i'm build a robot exactly like yours, here the code i'm using maybe it can helps



Code: [Select]
#include <Servo.h>          //Servo motor library. This is standard library
#include <NewPing.h>        //Ultrasonic sensor function library. You must install this library

//our L298N control pins
const int LeftMotorForward = 3;
const int LeftMotorBackward = 5;
const int RightMotorForward = 6;
const int RightMotorBackward = 11;

//sensor pins
#define trig_pin A0 //analog input 1
#define echo_pin A1 //analog input 2

#define maximum_distance 200
boolean goesForward = false;
int distance = 100;

NewPing sonar(trig_pin, echo_pin, maximum_distance); //sensor function
Servo servo_motor; //our servo name


void setup(){

  pinMode(RightMotorForward, OUTPUT);
  pinMode(LeftMotorForward, OUTPUT);
  pinMode(LeftMotorBackward, OUTPUT);
  pinMode(RightMotorBackward, OUTPUT);
 
  servo_motor.attach(10); //our servo pin

  servo_motor.write(115);
  delay(2000);
  distance = readPing();
  delay(100);
  distance = readPing();
  delay(100);
  distance = readPing();
  delay(100);
  distance = readPing();
  delay(100);
}

void loop(){

  int distanceRight = 0;
  int distanceLeft = 0;
  delay(50);

  if (distance <= 20){
    moveStop();
    delay(300);
    moveBackward();
    delay(400);
    moveStop();
    delay(300);
    distanceRight = lookRight();
    delay(300);
    distanceLeft = lookLeft();
    delay(300);

    if (distance >= distanceLeft){
      turnRight();
      moveStop();
    }
    else{
      turnLeft();
      moveStop();
    }
  }
  else{
    moveForward();
  }
    distance = readPing();
}

int lookRight(){ 
  servo_motor.write(50);
  delay(500);
  int distance = readPing();
  delay(100);
  servo_motor.write(115);
  return distance;
}

int lookLeft(){
  servo_motor.write(170);
  delay(500);
  int distance = readPing();
  delay(100);
  servo_motor.write(115);
  return distance;
  delay(100);
}

int readPing(){
  delay(70);
  int cm = sonar.ping_cm();
  if (cm==0){
    cm=250;
  }
  return cm;
}

void moveStop(){
 
  digitalWrite(RightMotorForward, LOW);
  digitalWrite(LeftMotorForward, LOW);
  digitalWrite(RightMotorBackward, LOW);
  digitalWrite(LeftMotorBackward, LOW);
}

void moveForward(){

  if(!goesForward){

    goesForward=true;
   
    analogWrite(LeftMotorForward, 90);
    analogWrite(RightMotorForward, 95);
 
    analogWrite(LeftMotorBackward, 0);
    analogWrite(RightMotorBackward, 0);
  }
}

void moveBackward(){

  goesForward=false;

  analogWrite(LeftMotorBackward, 90);
  analogWrite(RightMotorBackward, 95);
 
  analogWrite(LeftMotorForward, 0);
  analogWrite(RightMotorForward, 0);
 
}

void turnRight(){

  analogWrite(LeftMotorForward, 90);
  analogWrite(RightMotorBackward, 95);
 
  analogWrite(LeftMotorBackward, 0);
  analogWrite(RightMotorForward, 0);
 
  delay(450);
 
  analogWrite(LeftMotorForward, 90);
  analogWrite(RightMotorForward, 95);
 
  analogWrite(LeftMotorBackward, 0);
  analogWrite(RightMotorBackward, 0);
 
 
 
}

void turnLeft(){

  analogWrite(LeftMotorBackward, 90);
  analogWrite(RightMotorForward, 95);
 
  analogWrite(LeftMotorForward, 0);
  analogWrite(RightMotorBackward, 0);

  delay(450);
 
  digitalWrite(LeftMotorForward, 90);
  digitalWrite(RightMotorForward, 95);
 
  digitalWrite(LeftMotorBackward, 0);
  digitalWrite(RightMotorBackward, 0);
}

padawan

Hi

@jorpec thanks for the advice but I think I might finally have figured this out! ...mostly at least... I plan on making this little guy do more fun stuff later so I might use your suggestion. I saw you were making an R2D2 robot. I hope it works because that would just be so cute, and besides...it's Star Wars!

Ok so I tested the robot again and with the new code everything was still running backwards, but then both wheels started forwards when something got within 10 cm of the sensor. I think I just made a stupid stupid stupid mistake. I probably just mixed up which is left and which is right motor on the motor driver, because I switched the motors and now everything runs right. Thanks everyone for all the help especially to make both the motors react to the sensor. Does that seem like a plausible solution to you all?

But then my weird setup thing started running backwards so I added a few digitalWrite()'s (is that how you make that plural?) to the setup. Those have an "added2" comment. It kept doing the exact same thing so I added more digitalWrite()'s with an "added3" comment, but that didn't change anything. My "motor switching" solution doesn't explain this! Any ideas? I'm probably missing something obvious, again! Here's the revised code:
Code: [Select]
[code]
const int pwm1 = 11;//left motor 
const int in1 = 10;//left backward
const int in2 = 9;//left forward
const int pwm2 = 6;//right motor
const int in3 = 5;// right backward
const int in4 = 4;// right forward
const int led = 13;
const int pingPin = 7;

 
void setup() {
  pinMode (pwm1, OUTPUT);
  pinMode(in1, OUTPUT);
  pinMode(in2, OUTPUT);
  pinMode(pwm2, OUTPUT);
  pinMode(in3, OUTPUT);
  pinMode(in4, OUTPUT);
  pinMode(led, OUTPUT);
 
  digitalWrite(in1, LOW);//added2
  digitalWrite(pwm1, LOW);//added3, is this necessary?
  digitalWrite(in2, HIGH);//left motor forward
  digitalWrite(pwm1, HIGH);
  digitalWrite(in3, LOW);//added2
  digitalWrite(pwm2, LOW);//added3, is this necessary?
  digitalWrite(in4, HIGH);//right motor forward
  digitalWrite(pwm2, HIGH);
  delay(1000);
  digitalWrite(in2, LOW);
  digitalWrite(pwm1, LOW);
  digitalWrite(in4, LOW);
  digitalWrite(pwm2, LOW);
  delay(1000);
  digitalWrite(in1, HIGH);// left motor backwards
  digitalWrite(pwm1, HIGH);
  digitalWrite(in3, HIGH);//right motor backwards
  digitalWrite(pwm2, HIGH);
  delay(1000);
  digitalWrite(in1, LOW);
  digitalWrite(pwm1, LOW);
  digitalWrite(in3, LOW);
  digitalWrite(pwm2, LOW);
 
  digitalWrite(led, HIGH);//led on for five seconds
  delay(5000);
  digitalWrite(led, LOW);// led off
  Serial.begin(9600);// stops doing what i want it to do here
}
void loop(){
  digitalWrite(in1, LOW);//added
  digitalWrite(pwm1, LOW);//added
  digitalWrite(in3, LOW);//added
  digitalWrite(pwm2, LOW);//added
  digitalWrite(in2, HIGH);//left motor forwards
  digitalWrite(pwm1, HIGH);
  digitalWrite(in4, HIGH);//right motor forwards
  digitalWrite(pwm2, HIGH);
  uint8_t i;

  long duration, inches, cm;
  pinMode(pingPin, OUTPUT);
  digitalWrite(pingPin, LOW);
  delayMicroseconds(2);
  digitalWrite(pingPin,HIGH);
  delayMicroseconds(5);
  digitalWrite(pingPin, LOW);
 
  pinMode(pingPin, INPUT);
  duration = pulseIn(pingPin, HIGH);
  cm = microsecondsToCentimeters(duration);
  Serial.print(cm);
  Serial.print("cm");
  Serial.println();
  if (cm >10){
    digitalWrite(in1,LOW);//added
    digitalWrite(in2, LOW);//left motor, backwards
    digitalWrite(pwm1, LOW);
    digitalWrite(in1, HIGH);
    digitalWrite(pwm1, HIGH);

  //for (i=0; i<255; i++){ i can't even remember why i commented this out. any ideas?
 // delay(10);}
  digitalWrite(in3,LOW);//added
  digitalWrite(in4, LOW);// right motor, backwards
  digitalWrite(pwm2, LOW);
  digitalWrite(in3, HIGH);
  digitalWrite(pwm2, HIGH);
  for (i=0; i<255; i++){
  delay(10);}
 
digitalWrite(in3, LOW);//start both motors forward again
digitalWrite(pwm2,LOW);// added is this one of the parts you were talking about?
digitalWrite(in4, HIGH);
digitalWrite(pwm2, HIGH);//added
digitalWrite(in2, LOW);
digitalWrite(pwm1,LOW);//added
digitalWrite(in1, HIGH);
digitalWrite(pwm1, HIGH);//added
delay(30);
  }
  else{
    digitalWrite(in1, LOW);//added
    digitalWrite(pwm1, LOW);//added
    digitalWrite(in2, HIGH);//keep going straight
    digitalWrite(pwm1, HIGH);
    digitalWrite(in3, LOW);//added
    digitalWrite(pwm2, LOW);//added
    digitalWrite(in4, HIGH);
    digitalWrite(pwm2, HIGH);
  }
  }
  long microsecondsToCentimeters(long microseconds)
  {
 
    return microseconds / 29/ 2;}
 
 
[/code]
Thanks again.
-p.

jorpec

I see you are using the same motor driver as i, and I had the same motor problems as you, the motors move backwards on the first run but i realize the wires on the motors was in reverse, then another user of this forum suggested that i keep the pwm1 and pwn2 jumper on the motor driver and just deal only with the in1, in2 ... to make the motor move

I used this code to test the motors to see if the my problem was on the motors wiring or code, maybe you can check with this very simple code where is the problem (just need to change the pins number)


Code: [Select]
// connect motor controller pins to Arduino digital pins
// motor one
int enA = 10;
int in1 = 9;
int in2 = 8;
// motor two
int enB = 5;
int in3 = 7;
int in4 = 6;
void setup()
{
  // set all the motor control pins to outputs
  pinMode(enA, OUTPUT);
  pinMode(enB, OUTPUT);
  pinMode(in1, OUTPUT);
  pinMode(in2, OUTPUT);
  pinMode(in3, OUTPUT);
  pinMode(in4, OUTPUT);
}
void demoOne()
{
  // this function will run the motors in both directions at a fixed speed
  // turn on motor A
  digitalWrite(in1, HIGH);
  digitalWrite(in2, LOW);
  // set speed to 200 out of possible range 0~255
  analogWrite(enA, 200);
  // turn on motor B
  digitalWrite(in3, HIGH);
  digitalWrite(in4, LOW);
  // set speed to 200 out of possible range 0~255
  analogWrite(enB, 200);
  delay(2000);
  // now change motor directions
  digitalWrite(in1, LOW);
  digitalWrite(in2, HIGH); 
  digitalWrite(in3, LOW);
  digitalWrite(in4, HIGH);
  delay(2000);
  // now turn off motors
  digitalWrite(in1, LOW);
  digitalWrite(in2, LOW); 
  digitalWrite(in3, LOW);
  digitalWrite(in4, LOW);
}
void demoTwo()
{
  // this function will run the motors across the range of possible speeds
  // note that maximum speed is determined by the motor itself and the operating voltage
  // the PWM values sent by analogWrite() are fractions of the maximum speed possible
  // by your hardware
  // turn on motors
  digitalWrite(in1, LOW);
  digitalWrite(in2, HIGH); 
  digitalWrite(in3, LOW);
  digitalWrite(in4, HIGH);
  // accelerate from zero to maximum speed
  for (int i = 0; i < 256; i++)
  {
    analogWrite(enA, i);
    analogWrite(enB, i);
    delay(20);
  }
  // decelerate from maximum speed to zero
  for (int i = 255; i >= 0; --i)
  {
    analogWrite(enA, i);
    analogWrite(enB, i);
    delay(20);
  }
  // now turn off motors
  digitalWrite(in1, LOW);
  digitalWrite(in2, LOW); 
  digitalWrite(in3, LOW);
  digitalWrite(in4, LOW); 
}
void loop()
{
  demoOne();
  delay(1000);
  demoTwo();
  delay(1000);
}

padawan

Hi
@jorpec thanks for the advice. I ran that test and the first demo ran fine but the second one ran backwards, with the left motor starting  half a second or so before the right. I changed the 9v battery powering the Uno to see if that would change anything but it didn't. So I tried changing my 6 AA batteries as well as the 9v, no change. I'm wondering if there might be something wrong with one of the motors, or maybe the motor driver. I don't think there could be a problem with the Uno, but it's still possible so I'll troubleshoot and post the results.

I also saw that my robot was running too fast to react to the sensor in time so I changed the digitalWrite(pwm?, HIGH or LOW); to analogWrite(pwm?, 100 or 0); and that solved the speed problem, though my robot still hits walls before it backs away from them. But my left motor is also for whatever reason running better than the right one, making the robot turn to the right slightly when it should be going straight. Is there anything in the code that could be making it do that? Or do I need to add more batteries, or is my right motor faulty? Any ideas?

I also decided to change the distance the robot would back off at to 20 cm instead of 10 because I'm running it in a kitchen where the sensor sometimes doesn't "see" the ledge where the lower cupboards jut out over the floor and it works a bit better this way I think. It still hits stuff before backing off though. Any ideas? Thanks in advance! New code:
Code: [Select]
const int pwm1 = 11;//left motor 
const int in1 = 10;//left backward
const int in2 = 9;//left forward
const int pwm2 = 6;//right motor
const int in3 = 5;// right backward
const int in4 = 4;// right forward
const int led = 13;
const int pingPin = 7;

 
void setup() {
  pinMode (pwm1, OUTPUT);
  pinMode(in1, OUTPUT);
  pinMode(in2, OUTPUT);
  pinMode(pwm2, OUTPUT);
  pinMode(in3, OUTPUT);
  pinMode(in4, OUTPUT);
  pinMode(led, OUTPUT);
 
  digitalWrite(in1, LOW);//added2
  analogWrite(pwm1, 0);//added3, is this necessary? speed control
  digitalWrite(in2, HIGH);//left motor forward
  analogWrite(pwm1, 100);//speed control
  digitalWrite(in3, LOW);//added2
  analogWrite(pwm2, 0);//added3, is this necessary? speed control
  digitalWrite(in4, HIGH);//right motor forward
  analogWrite(pwm2, 100);//speed control
  delay(1000);
  digitalWrite(in2, LOW);
  analogWrite(pwm1, 0);//speed control
  digitalWrite(in4, LOW);
  analogWrite(pwm2, 0);//speed control
  delay(1000);
  digitalWrite(in1, HIGH);// left motor backwards
  analogWrite(pwm1, 100);//speed control
  digitalWrite(in3, HIGH);//right motor backwards
  analogWrite(pwm2, 100);//speed control
  delay(1000);
  digitalWrite(in1, LOW);
  analogWrite(pwm1, 0);//speed control
  digitalWrite(in3, LOW);
  analogWrite(pwm2, 0);//speed control
 
  digitalWrite(led, HIGH);//led on for five seconds
  delay(5000);
  digitalWrite(led, LOW);// led off
  Serial.begin(9600);// stops doing what i want it to do here
}
void loop(){
  digitalWrite(in1, LOW);//added
  analogWrite(pwm1, 0);//added, speed control
  digitalWrite(in3, LOW);//added
  analogWrite(pwm2, 0);//added, speed control
  digitalWrite(in2, HIGH);//left motor forwards
  analogWrite(pwm1, 100);//speed control
  digitalWrite(in4, HIGH);//right motor forwards
  analogWrite(pwm2, 100);//speed control
  uint8_t i;

  long duration, inches, cm;
  pinMode(pingPin, OUTPUT);
  digitalWrite(pingPin, LOW);
  delayMicroseconds(2);
  digitalWrite(pingPin,HIGH);
  delayMicroseconds(5);
  digitalWrite(pingPin, LOW);
 
  pinMode(pingPin, INPUT);
  duration = pulseIn(pingPin, HIGH);
  cm = microsecondsToCentimeters(duration);
  Serial.print(cm);
  Serial.print("cm");
  Serial.println();
  if (cm >20){
    digitalWrite(in1,LOW);//added
    digitalWrite(in2, LOW);//left motor, backwards
    analogWrite(pwm1, 0);//speed control
    digitalWrite(in1, HIGH);
    analogWrite(pwm1, 100);//speed control

  //for (i=0; i<255; i++){ i can't even remember why i commented this out. any ideas?
 // delay(10);}
  digitalWrite(in3,LOW);//added
  digitalWrite(in4, LOW);// right motor, backwards
  analogWrite(pwm2, 0);//speed control
  digitalWrite(in3, HIGH);
  analogWrite(pwm2, 100);//speed control
  for (i=0; i<255; i++){
  delay(10);}
 
digitalWrite(in3, LOW);//start both motors forward again
analogWrite(pwm2, 0);// added, is this one of the parts you were talking about? speed control
digitalWrite(in4, HIGH);
analogWrite(pwm2, 100);//added, speed control
digitalWrite(in2, LOW);
analogWrite(pwm1,0);//added, speed control
digitalWrite(in1, HIGH);
analogWrite(pwm1, 100);//added, speed control
delay(30);
  }
  else{
    digitalWrite(in1, LOW);//added
    analogWrite(pwm1, 0);//added, speed control
    digitalWrite(in2, HIGH);//keep going straight
    analogWrite(pwm1, 100);//speed control
    digitalWrite(in3, LOW);//added
    analogWrite(pwm2, 0);//added, speed control
    digitalWrite(in4, HIGH);
    analogWrite(pwm2, 100);//speed control
  }
  }
  long microsecondsToCentimeters(long microseconds)
  {
 
    return microseconds / 29/ 2;}
 
 

-p.

jorpec

About the "left motor starting  half a second or so before the right" the same happens to me, i guess the motors are not exactly the same, i solved by on analogWrite on the weak motor put an higher value, about the speed try to lower the pwm value ( in my case to the robot goes straight forward i need to put 95 in the left motor and 75 on the right motor)

Also check the arduino pins that cant handle the analogWrite, i made that mistake (in my case the analogWrite  only works on pins nÂș 3, 5, 6, 11) 

About "It still hits stuff before backing off though. Any ideas?" again the same happens to me so i'm thinking to change the ultrasonic sensors by InfraRed Leds

padawan

Hi
@jorpec thanks again. I'll try the setting one motor higher trick. I am using pins that  do work with analogWrite() .
-p.

padawan

Wow I really have to stop getting on the forum at midnight :smiley-sleep: . @jorpec I just looked at your other topic concerning using an IR sensor to let the robot "see" things off to the side and not hit them and I think I'll have to add a servo or something to turn the Ultrasonic sensor to "see" more, something like "check right, left and forwards an if there's nothing within 20 cm go forwards." Of course that is a lot simpler than what I'll actually have to program it to do but you probably get the idea. Since I haven't tried to code this yet I don't know if I'll have to ask the community for help but I'm just putting this information out there in case anyone wonders where that new snippet of code came from. Again sorry if I'm incoherent, it's late.
-p.

jorpec

My robot has a servo attached to the ultrasonic sensor, but only moves left or right when it "see" something, in your case if the servo moving left and right constantly i guess it will "see" obstacles on a wider angle

padawan

Hi
I changed all pwm2 values from 100 to 125 and that solved the motor speed difference problem.

I also just changed the time the LED was on during setup from 5 seconds to 2 seconds. New code:
   
Code: [Select]
const int pwm1 = 11;//left motor 
const int in1 = 10;//left backward
const int in2 = 9;//left forward
const int pwm2 = 6;//right motor
const int in3 = 5;// right backward
const int in4 = 4;// right forward
const int led = 13;
const int pingPin = 7;

 
void setup() {
  pinMode (pwm1, OUTPUT);
  pinMode(in1, OUTPUT);
  pinMode(in2, OUTPUT);
  pinMode(pwm2, OUTPUT);
  pinMode(in3, OUTPUT);
  pinMode(in4, OUTPUT);
  pinMode(led, OUTPUT);
 
  digitalWrite(in1, LOW);//added2
  analogWrite(pwm1, 0);//added3, is this necessary? speed control
  digitalWrite(in2, HIGH);//left motor forward
  analogWrite(pwm1, 100);//speed control
  digitalWrite(in3, LOW);//added2
  analogWrite(pwm2, 0);//added3, is this necessary? speed control
  digitalWrite(in4, HIGH);//right motor forward
  analogWrite(pwm2, 125);//speed control
  delay(1000);
  digitalWrite(in2, LOW);
  analogWrite(pwm1, 0);//speed control
  digitalWrite(in4, LOW);
  analogWrite(pwm2, 0);//speed control
  delay(1000);
  digitalWrite(in1, HIGH);// left motor backwards
  analogWrite(pwm1, 100);//speed control
  digitalWrite(in3, HIGH);//right motor backwards
  analogWrite(pwm2, 125);//speed control
  delay(1000);
  digitalWrite(in1, LOW);
  analogWrite(pwm1, 0);//speed control
  digitalWrite(in3, LOW);
  analogWrite(pwm2, 0);//speed control
 
  digitalWrite(led, HIGH);//led on for five seconds
  delay(2000);//changed from 5 sec to 2 sec
  digitalWrite(led, LOW);// led off
  Serial.begin(9600);// stops doing what i want it to do here
}
void loop(){
  digitalWrite(in1, LOW);//added
  analogWrite(pwm1, 0);//added, speed control
  digitalWrite(in3, LOW);//added
  analogWrite(pwm2, 0);//added, speed control
  digitalWrite(in2, HIGH);//left motor forwards
  analogWrite(pwm1, 100);//speed control
  digitalWrite(in4, HIGH);//right motor forwards
  analogWrite(pwm2, 125);//speed control
  uint8_t i;

  long duration, inches, cm;
  pinMode(pingPin, OUTPUT);
  digitalWrite(pingPin, LOW);
  delayMicroseconds(2);
  digitalWrite(pingPin,HIGH);
  delayMicroseconds(5);
  digitalWrite(pingPin, LOW);
 
  pinMode(pingPin, INPUT);
  duration = pulseIn(pingPin, HIGH);
  cm = microsecondsToCentimeters(duration);
  Serial.print(cm);
  Serial.print("cm");
  Serial.println();
  if (cm >20){
    digitalWrite(in1,LOW);//added
    digitalWrite(in2, LOW);//left motor, backwards
    analogWrite(pwm1, 0);//speed control
    digitalWrite(in1, HIGH);
    analogWrite(pwm1, 100);//speed control

  //for (i=0; i<255; i++){ i can't even remember why i commented this out. any ideas?
 // delay(10);}
  digitalWrite(in3,LOW);//added
  digitalWrite(in4, LOW);// right motor, backwards
  analogWrite(pwm2, 0);//speed control
  digitalWrite(in3, HIGH);
  analogWrite(pwm2, 125);//speed control
  for (i=0; i<255; i++){
  delay(10);}
 
digitalWrite(in3, LOW);//start both motors forward again
analogWrite(pwm2, 0);// added, is this one of the parts you were talking about? speed control
digitalWrite(in4, HIGH);
analogWrite(pwm2, 125);//added, speed control
digitalWrite(in2, LOW);
analogWrite(pwm1,0);//added, speed control
digitalWrite(in1, HIGH);
analogWrite(pwm1, 100);//added, speed control
delay(30);
  }
  else{
    digitalWrite(in1, LOW);//added
    analogWrite(pwm1, 0);//added, speed control
    digitalWrite(in2, HIGH);//keep going straight
    analogWrite(pwm1, 100);//speed control
    digitalWrite(in3, LOW);//added
    analogWrite(pwm2, 0);//added, speed control
    digitalWrite(in4, HIGH);
    analogWrite(pwm2, 125);//speed control
  }
  }
  long microsecondsToCentimeters(long microseconds)
  {
 
    return microseconds / 29/ 2;}
 
 

-p.

padawan

Hi
Sorry for not getting around to this sooner but I am going to declare this solved. I realized as I learned more about Arduino that I don't know what the code I was basing mine off of was trying to do, because it literally makes the robot back off and then run into the wall. Or at least my changed up one did, probably a part I missed. I will keep working on my project, incorporating turning and probably more sensors. So the issues this topic was on are solved (that's not saying that there won't be more) though my robot is far from done! Thanks everyone for your time and help!
-p.

Go Up