Motor control logic problem

neil28461 · May 23, 2024, 8:34pm

This sketch works great, except when it runs into an object when it should back up and look around, but does not. It works fine otherwise, can someone see the problem, i dont?
Using R3, L298N motor shield.
//4wd analog ping and servo

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

const  int LeftMotorForward = 6;
const int LeftMotorBackward = 11;
const int RightMotorForward  = 3;
const int RightMotorBackward = 5;

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

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

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

void setup(){

  Serial.begin(9600); //see ping results

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

  servo_motor.write(90);
  delay(500);
  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);
  Serial.print(distance);
  Serial.println("cm");
  if (distance <= 100){
    moveStop();
    delay(300);
    moveBackward();
    delay(300);
    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(45);
  delay(300);
  int distance =  readPing();
  delay(100);
  servo_motor.write(90);
  return distance;
}

int  lookLeft(){
  servo_motor.write(135);
  delay(300);
  int distance = readPing();
  delay(100);
  servo_motor.write(90);
  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;
    
    digitalWrite(LeftMotorForward,  HIGH);
    digitalWrite(RightMotorForward, HIGH);
  
    digitalWrite(LeftMotorBackward,  LOW);
    digitalWrite(RightMotorBackward, LOW); 
  }
}

void moveBackward(){

  goesForward=false;

  digitalWrite(LeftMotorBackward, HIGH);
  digitalWrite(RightMotorBackward,  HIGH);
  
  delay(1000);

  digitalWrite(LeftMotorForward, LOW);
  digitalWrite(RightMotorForward,  LOW);
  
}

void turnRight(){

  digitalWrite(LeftMotorForward,  HIGH);
  digitalWrite(RightMotorBackward, HIGH);
  
  digitalWrite(LeftMotorBackward,  LOW);
  digitalWrite(RightMotorForward, LOW);
  
  delay(500);
  
  digitalWrite(LeftMotorForward, HIGH);
  digitalWrite(RightMotorForward, HIGH);
  
  digitalWrite(LeftMotorBackward, LOW);
  digitalWrite(RightMotorBackward,  LOW);
 
  
  
}

void turnLeft(){

  digitalWrite(LeftMotorBackward,  HIGH);
  digitalWrite(RightMotorForward, HIGH);
  
  digitalWrite(LeftMotorForward,  LOW);
  digitalWrite(RightMotorBackward, LOW);

  delay(500);
  
  digitalWrite(LeftMotorForward, HIGH);
  digitalWrite(RightMotorForward, HIGH);
  
  digitalWrite(LeftMotorBackward, LOW);
  digitalWrite(RightMotorBackward,  LOW);
}

LarryD · May 23, 2024, 9:07pm

Be a bit careful, you have a global and a local distance variable.

UKHeliBob · May 23, 2024, 9:08pm

What does it do ?

neil28461 · May 23, 2024, 10:40pm

It runs into an object, backs up, goes forward , backs up, goes forward and repeats. Sensor is moving left to right, pinging and seeing its still blocked and sending back distance ( im using blue tooth to phone and can see the distance) but i want it to stop, backup, scan left and right and decide which direction to go.

xfpd · May 24, 2024, 2:48am

I would call that a failure, because it is an object-avoidance car. Let's clean it up.

The servo pin needs a name. Add this to the definitions...

#define servo_pin 8 // our servo pin

... and change this:

  servo_motor.attach(8); //our servo pin

to this

  servo_motor.attach(servo_pin); //our servo pin

These lines in setup() do nothing...

  delay(500); // delete this line
  distance = readPing(); // delete this line
  delay(100); // delete this line
  distance = readPing(); // delete this line
  delay(100); // delete this line
  distance = readPing(); // delete this line
  delay(100); // delete this line
  distance = readPing(); // delete this line
  delay(100); // delete this line

You have these two...

  int distanceRight = 0;
  int distanceLeft = 0;

... but nothing for forward, so add this...

  int distanceFront = 0;

This is not needed...

    delay(50); // delete this line

Again, the sketch never looks forward, so add this...

    distanceForward = lookForward();
    delay(300);

... and add the lookForward() function...

int lookForward() {
}

Why back up? Remove these...

   moveBackward();
   delay(300);
   moveStop();
   delay(300);

This has no direction... remove it.

    if (distance >= distanceLeft) {

This is not used... remove references to it.

boolean goesForward = false;

This is not how to turn left... left backward and right forward are the commands.

void turnLeft() {
  digitalWrite(LeftMotorBackward, HIGH);
  digitalWrite(RightMotorForward, HIGH);
  digitalWrite(LeftMotorForward, LOW);
  digitalWrite(RightMotorBackward, LOW);
  delay(500);
  digitalWrite(LeftMotorForward, HIGH);
  digitalWrite(RightMotorForward, HIGH);
  digitalWrite(LeftMotorBackward, LOW);
  digitalWrite(RightMotorBackward, LOW);
}

The backup, forward, backup, forward was due to the sketch being written to do that. Try this update to your sketch.

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

#define servo_pin 8;
Servo servo_motor; //our servo  name

const int LeftMotorForward = 6;
const int LeftMotorBackward = 11;
const int RightMotorForward  = 3;
const int RightMotorBackward = 5;

#define trig_pin  A1 //analog input 1
#define echo_pin A2 //analog input 2
#define maximum_distance  250
int distance = 300;
NewPing sonar(trig_pin,  echo_pin, maximum_distance); //sensor function

void setup() {
  Serial.begin(9600); //see ping results
  pinMode(RightMotorForward,  OUTPUT);
  pinMode(LeftMotorForward,   OUTPUT);
  pinMode(LeftMotorBackward,  OUTPUT);
  pinMode(RightMotorBackward, OUTPUT);

  servo_motor.attach(8); // attach servo to servo pin
  servo_motor.write(90); // point forward
}

void loop() {
  int distanceRight = 0;
  int distanceLeft = 0;
  int distanceFront = 0;

  if (distance <= 100) {
    moveStop();
    delay(300);
    distanceRight = lookRight();
    Serial.print(" RIGHT: ");
    Serial.print(distanceRight);
    delay(300);

    distanceLeft  = lookLeft();
    Serial.print(" LEFT: ");
    Serial.print(distanceLeft);
    delay(300);

    if (distance >= distanceLeft) {
      turnRight();
      moveStop();
    }
    else {
      turnLeft();
      moveStop();
    }
  }
  else {
    moveForward();
  }
  distance = readPing();
    Serial.print(" FORWARD: ");
    Serial.println(distance);
}

int lookRight() {
  servo_motor.write(30);
  delay(300);
  int distance =  readPing();
  servo_motor.write(90);
  delay(300);
  return distance;
}

int lookLeft() {
  servo_motor.write(150);
  delay(300);
  int distance = readPing();
  servo_motor.write(90);
  delay(300);
  return distance;
}

int readPing() {
  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() {
  digitalWrite(LeftMotorForward, HIGH);
  digitalWrite(RightMotorForward, HIGH);
  delay(1000);
  moveStop();
}

void moveBackward() {
  digitalWrite(LeftMotorBackward, HIGH);
  digitalWrite(RightMotorBackward, HIGH);
  delay(1000);
  moveStop();
}

void turnRight() {
  digitalWrite(LeftMotorForward, HIGH);
  digitalWrite(LeftMotorBackward, LOW);
  digitalWrite(RightMotorForward, LOW);
  digitalWrite(RightMotorBackward, HIGH);
  delay(500);
  moveStop();
}

void turnLeft() {
  digitalWrite(LeftMotorForward, LOW);
  digitalWrite(LeftMotorBackward, HIGH);
  digitalWrite(RightMotorForward, HIGH);
  digitalWrite(RightMotorBackward, LOW);
  delay(500);
  moveStop();
}

diagram.json file for wokwi.com

{
  "version": 1,
  "author": "Anonymous maker",
  "editor": "wokwi",
  "parts": [
    { "type": "wokwi-arduino-nano", "id": "nano", "top": 0, "left": 0, "attrs": {} },
    {
      "type": "wokwi-led",
      "id": "led1",
      "top": -78,
      "left": 121.8,
      "rotate": 90,
      "attrs": { "color": "green" }
    },
    {
      "type": "wokwi-led",
      "id": "led2",
      "top": -49.2,
      "left": 121.8,
      "rotate": 90,
      "attrs": { "color": "green" }
    },
    {
      "type": "wokwi-led",
      "id": "led3",
      "top": -78,
      "left": -17.8,
      "rotate": 270,
      "attrs": { "color": "red", "flip": "1" }
    },
    {
      "type": "wokwi-led",
      "id": "led4",
      "top": -49.2,
      "left": -17.8,
      "rotate": 270,
      "attrs": { "color": "red", "flip": "1" }
    },
    { "type": "wokwi-hc-sr04", "id": "ultrasonic1", "top": -161.7, "left": -23.3, "attrs": {} },
    { "type": "wokwi-servo", "id": "servo1", "top": -50, "left": 182.4, "attrs": {} }
  ],
  "connections": [
    [ "nano:6", "led3:A", "green", [ "v0" ] ],
    [ "nano:11", "led4:A", "green", [ "v0" ] ],
    [ "nano:3", "led1:A", "green", [ "v0" ] ],
    [ "nano:5", "led2:A", "green", [ "v0" ] ],
    [ "nano:GND.2", "led4:C", "black", [ "v-14.4", "h-29.3", "v-19.6" ] ],
    [ "nano:GND.2", "led2:C", "black", [ "v-14.4", "h-29.3", "v-19.6" ] ],
    [ "nano:GND.2", "led3:C", "black", [ "v-14.4", "h-29.3", "v-48.4" ] ],
    [ "nano:GND.2", "led1:C", "black", [ "v-14.4", "h-29.3", "v-48.4" ] ],
    [ "nano:A1", "ultrasonic1:TRIG", "gold", [ "v0" ] ],
    [ "nano:A2", "ultrasonic1:ECHO", "orange", [ "v0" ] ],
    [ "nano:5V", "ultrasonic1:VCC", "red", [ "v-14.4", "h-67.7" ] ],
    [ "nano:GND.1", "ultrasonic1:GND", "black", [ "v-24", "h-67.7" ] ],
    [ "nano:8", "servo1:PWM", "violet", [ "v0" ] ],
    [ "nano:5V", "servo1:V+", "red", [ "v0" ] ],
    [ "nano:GND.1", "servo1:GND", "black", [ "v0" ] ]
  ],
  "dependencies": {}
}

neil28461 · May 24, 2024, 9:35pm

Thanks so much for corrected sketch. It still does not go backwards. The output is below, i want it to go backwards if forward less than 10cm, can you help? It does turn fine, so the motors will run in reverse.
FORWARD: 7
RIGHT: 11 LEFT: 232 FORWARD: 250
FORWARD: 8
RIGHT: 168 LEFT: 5 FORWARD: 5
RIGHT: 7 LEFT: 6 FORWARD: 4
RIGHT: 167 LEFT: 10 FORWARD: 7
RIGHT: 13 LEFT: 16 FORWARD: 8
RIGHT: 10 LEFT: 6 FORWARD: 250
FORWARD: 250
FORWARD: 250
FORWARD: 250

xfpd · May 24, 2024, 10:35pm

Your original sketch does not contain "distance < 10" or "distance <= 10".

You can add "if (distance < 10)... moveBackward();" wherever you want.

  distance = readPing();
  Serial.print(" FORWARD: ");
  Serial.println(distance);
  if (distance <= 10) {
    moveBackward();
    delay(500);
  }

Here is the sketch from post #5 plus the little bit from this post:

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

#define servo_pin 8;
Servo servo_motor; //our servo  name

const int LeftMotorForward = 6;
const int LeftMotorBackward = 11;
const int RightMotorForward  = 3;
const int RightMotorBackward = 5;

#define trig_pin  A1 //analog input 1
#define echo_pin A2 //analog input 2
#define maximum_distance  250
int distance = 300;
NewPing sonar(trig_pin,  echo_pin, maximum_distance); //sensor function

void setup() {
  Serial.begin(9600); //see ping results
  pinMode(RightMotorForward,  OUTPUT);
  pinMode(LeftMotorForward,   OUTPUT);
  pinMode(LeftMotorBackward,  OUTPUT);
  pinMode(RightMotorBackward, OUTPUT);

  servo_motor.attach(8); // attach servo to servo pin
  servo_motor.write(90); // point forward
}

void loop() {
  int distanceRight = 0;
  int distanceLeft = 0;
  int distanceFront = 0;

  if (distance <= 100) {
    moveStop();
    delay(300);
    distanceRight = lookRight();
    Serial.print(" RIGHT: ");
    Serial.print(distanceRight);
    delay(300);

    distanceLeft  = lookLeft();
    Serial.print(" LEFT: ");
    Serial.print(distanceLeft);
    delay(300);

    if (distance >= distanceLeft) {
      turnRight();
      moveStop();
    }
    else {
      turnLeft();
      moveStop();
    }
  }
  else {
    moveForward();
  }
  distance = readPing();
  Serial.print(" FORWARD: ");
  Serial.println(distance);
  if (distance <= 10) {
    moveBackward();
    delay(500);
  }
}

int lookRight() {
  servo_motor.write(30);
  delay(300);
  int distance =  readPing();
  servo_motor.write(90);
  delay(300);
  return distance;
}

int lookLeft() {
  servo_motor.write(150);
  delay(300);
  int distance = readPing();
  servo_motor.write(90);
  delay(300);
  return distance;
}

int readPing() {
  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() {
  digitalWrite(LeftMotorForward, HIGH);
  digitalWrite(RightMotorForward, HIGH);
  delay(1000);
  moveStop();
}

void moveBackward() {
  digitalWrite(LeftMotorBackward, HIGH);
  digitalWrite(RightMotorBackward, HIGH);
  delay(1000);
  moveStop();
}

void turnRight() {
  digitalWrite(LeftMotorForward, HIGH);
  digitalWrite(LeftMotorBackward, LOW);
  digitalWrite(RightMotorForward, LOW);
  digitalWrite(RightMotorBackward, HIGH);
  delay(500);
  moveStop();
}

void turnLeft() {
  digitalWrite(LeftMotorForward, LOW);
  digitalWrite(LeftMotorBackward, HIGH);
  digitalWrite(RightMotorForward, HIGH);
  digitalWrite(RightMotorBackward, LOW);
  delay(500);
  moveStop();
}

system · November 20, 2024, 10:35pm

This topic was automatically closed 180 days after the last reply. New replies are no longer allowed.

Topic		Replies	Views
Trying to write a unique obstacle avoiding car Programming	27	297	November 2, 2025
Obstacle Avoiding Robot Programming	29	347	July 6, 2025
Obstacle Avoiding Car Please Help General Guidance	83	4249	November 28, 2024
Robot that navigate himself automatically Programming	89	448	October 15, 2025
Como hacer Carro que EVITA OBSTÁCULOS Español	7	201	May 13, 2025

Motor control logic problem

Related topics