Code not working on actual project, problems withe programming itself?

Hello,
I am relatively new to Arduino and C++, even though I do have some experience in Java. However, I recently started a project with a motor drive and ultrasonic sensors and since it’s my first time using Arduino I used a code I found online and updated it to the current libraries. Even though my code has no apparent errors, it doesn’t work on the actual Arduino Uno. I was wondering if anyone can see what was wrong and what is causing the robot to not move. In respect to the motors, sensor, and servo, they all work separately but after connecting everything and applying the code only the servo motor shifts slightly.

#include <Adafruit_MotorShield.h>  
#include <NewPing.h>
#include <Servo.h> 

#define TRIG_PIN A0 
#define ECHO_PIN A1 
#define MAX_DISTANCE 200 
#define MAX_SPEED 190 // sets speed of DC  motors
#define MAX_SPEED_OFFSET 20

NewPing sonar(TRIG_PIN, ECHO_PIN, MAX_DISTANCE); 

Adafruit_MotorShield AFMS = Adafruit_MotorShield();

Adafruit_DCMotor *motor1 = AFMS.getMotor(1);
Adafruit_DCMotor *motor2 = AFMS.getMotor(2);
Adafruit_DCMotor *motor3 = AFMS.getMotor(3);
Adafruit_DCMotor *motor4 = AFMS.getMotor(4);
Servo myservo;   

boolean goesForward=false;
int distance = 100;
int speedSet = 0;

void setup() {

 AFMS.begin(1000);
 Serial.begin(9600);
 myservo.attach(10);  
 myservo.write(110); 
 delay(2000);
 distance = readPing();
 delay(100);
 distance = readPing();
 delay(100);
 distance = readPing();
 delay(100);
 distance = readPing();
 delay(100);
 
 
 
}

void loop() {
int distanceR = 0;
int distanceL =  0;
delay(40);
int distance = readPing();

if(distance<=15)
{
 moveStop();
 delay(100);
 moveBackward();
 delay(300);
 moveStop();
 delay(200);
 distanceR = lookRight();
 delay(200);
 distanceL = lookLeft();
 delay(200);

 if(distanceR>=distanceL)
 {
   turnRight();
   moveStop();
 }else
 {
   turnLeft();
   moveStop();
 }
}else
{
 moveForward();
}

}

int lookRight()
{
   myservo.write(35); 
   delay(500);
   int distance = readPing();
   delay(100);
   myservo.write(110); 
   return distance;
}

int lookLeft()
{
   myservo.write(175); 
   delay(500);
   int distance = readPing();
   delay(100);
   myservo.write(110); 
   return distance;
   delay(100);
}

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

void moveStop() {
 motor1->run(RELEASE); 
 motor2->run(RELEASE);
 motor3->run(RELEASE);
 motor4->run(RELEASE);
 } 
 
void moveForward() {

if(goesForward==true)
 {
  
   motor1->run(FORWARD);      
   motor2->run(FORWARD);
   motor3->run(FORWARD); 
   motor4->run(FORWARD);     
  for (speedSet = 0; speedSet < MAX_SPEED; speedSet + 2) // slowly bring the speed up to avoid loading down the batteries too quickly
  {
   motor1->setSpeed(speedSet);
   motor2->setSpeed(speedSet);
   motor3->setSpeed(speedSet);
   motor4->setSpeed(speedSet);
   delay(5);
  }
 }
}

void moveBackward() {
   if(goesForward==false)
 {
   motor1->run(BACKWARD);      
   motor2->run(BACKWARD);
   motor3->run(BACKWARD);
   motor4->run(BACKWARD);  
 for (speedSet = 0; speedSet < MAX_SPEED; speedSet + 2) // slowly bring the speed up to avoid loading down the batteries too quickly
 {
   motor1->setSpeed(speedSet);
   motor2->setSpeed(speedSet);
   motor3->setSpeed(speedSet);
   motor4->setSpeed(speedSet);
   delay(5);
 }
   }
}  

void turnRight() {
 motor1->setSpeed(100);
 motor2->setSpeed(100);
 motor3->setSpeed(100);
 motor4->setSpeed(100);
 
 motor1->run(FORWARD);
 motor2->run(FORWARD);
 motor3->run(BACKWARD);
 motor4->run(BACKWARD);     
 delay(500);
 motor1->run(FORWARD);      
 motor2->run(FORWARD);
 motor3->run(FORWARD);
 motor4->run(FORWARD);      
} 

void turnLeft() {
  motor1->setSpeed(100);
 motor2->setSpeed(100);
 motor3->setSpeed(100);
 motor4->setSpeed(100);
 
 motor1->run(BACKWARD);     
 motor2->run(BACKWARD);  
 motor3->run(FORWARD);
 motor4->run(FORWARD);   
 delay(500);
 motor1->run(FORWARD);     
 motor2->run(FORWARD);
 motor3->run(FORWARD);
 motor4->run(FORWARD);
}

ARDUINO_OBSTACLE_AVOIDING_CAR.ino (3.86 KB)

Welcome to the Forum. Please read these posts:

General Guidance and How to use the Forum and

How to use this forum - please read..

You have added your sketch as an attachment. Unless the sketch is too large, it's better if you post your code (using code tags!), rather than attach it. When it's attached, we have to download it, create a folder then open your code in our IDE. And afterwards, the folder remains unless we navigate to the "Temp" folder and manually remove it. It's much easier to just view the code in your post.
Please edit your post and insert your code inside it, using code tags. The code tags make the code look

like this

when posting source code files. It makes it easier to read, and can be copied with a single mouse click. Also, if you don't do it, some of the character sequences in the code can be misinterpred by the forum code as italics or funny emoticons. The "Code: [Select]" feature allows someone to select the entire sketch so it can be easily copied and pasted into the IDE for testing.

Here is one problem:

 if(goesForward=true)

‘=’ is for assignment. ‘==’ is for comparison.

Same here:

    if(goesForward=false);

And you need to remove the semicolon. I assume you want this:

 if(goesForward==false)
 {
    motor1->run(BACKWARD);      
    motor2->run(BACKWARD);
    motor3->run(BACKWARD);
    motor4->run(BACKWARD);  
  for (speedSet = 0; speedSet < MAX_SPEED; speedSet + 2) // slowly bring the speed up to avoid loading down the batteries too quickly
  {
    motor1->setSpeed(speedSet);
    motor2->setSpeed(speedSet);
    motor3->setSpeed(speedSet);
    motor4->setSpeed(speedSet);
    delay(5);
  }
}

Thank you for letting me know about the code, I have updated it so downloads arent needed.
Even though I managed to fix the mistake it still doesn't move. I'm just confused about the reason why the DC motors are not responding at all and why the servo motor only shifts in very small increments. I thought it may be that the ultrasonic sensor isn't given the right commands to note the distance between the sensor and any object in front. Or maybe the distance is correct but my code isn't using that information to cause the motors to move. It seems to be stuck or missing information to continue with the program properly

We need to see your corrected code

Your wiring diagram might help also.

Ah yes! I’m sorry

#include <Adafruit_MotorShield.h>  
#include <NewPing.h>
#include <Servo.h> 

#define TRIG_PIN A0 
#define ECHO_PIN A1 
#define MAX_DISTANCE 200 
#define MAX_SPEED 190 // sets speed of DC  motors
#define MAX_SPEED_OFFSET 20

NewPing sonar(TRIG_PIN, ECHO_PIN, MAX_DISTANCE); 

Adafruit_MotorShield AFMS = Adafruit_MotorShield();

Adafruit_DCMotor *motor1 = AFMS.getMotor(1);
Adafruit_DCMotor *motor2 = AFMS.getMotor(2);
Adafruit_DCMotor *motor3 = AFMS.getMotor(3);
Adafruit_DCMotor *motor4 = AFMS.getMotor(4);
Servo myservo;   

boolean goesForward=false;
int distance = 100;
int speedSet = 0;

void setup() {

  AFMS.begin(1000);
  Serial.begin(9600);
  myservo.attach(10);  
  myservo.write(110); 
  delay(2000);
  distance = readPing();
  delay(100);
  distance = readPing();
  delay(100);
  distance = readPing();
  delay(100);
  distance = readPing();
  delay(100);
  
  
  
}

void loop() {
 int distanceR = 0;
 int distanceL =  0;
 delay(40);
 int distance = readPing();
 
 if(distance<=15)
 {
  moveStop();
  delay(100);
  moveBackward();
  delay(300);
  moveStop();
  delay(200);
  distanceR = lookRight();
  delay(200);
  distanceL = lookLeft();
  delay(200);

  if(distanceR>=distanceL)
  {
    turnRight();
    moveStop();
  }else
  {
    turnLeft();
    moveStop();
  }
 }else
 {
  moveForward();
 }
 
}

int lookRight()
{
    myservo.write(35); 
    delay(500);
    int distance = readPing();
    delay(100);
    myservo.write(110); 
    return distance;
}

int lookLeft()
{
    myservo.write(175); 
    delay(500);
    int distance = readPing();
    delay(100);
    myservo.write(110); 
    return distance;
    delay(100);
}

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

void moveStop() {
  motor1->run(RELEASE); 
  motor2->run(RELEASE);
  motor3->run(RELEASE);
  motor4->run(RELEASE);
  } 
  
void moveForward() {

 if(goesForward==true)
  {
   
    motor1->run(FORWARD);      
    motor2->run(FORWARD);
    motor3->run(FORWARD); 
    motor4->run(FORWARD);     
   for (speedSet = 0; speedSet < MAX_SPEED; speedSet + 2) // slowly bring the speed up to avoid loading down the batteries too quickly
   {
    motor1->setSpeed(speedSet);
    motor2->setSpeed(speedSet);
    motor3->setSpeed(speedSet);
    motor4->setSpeed(speedSet);
    delay(5);
   }
  }
}

void moveBackward() {
    if(goesForward==false)
  {
    motor1->run(BACKWARD);      
    motor2->run(BACKWARD);
    motor3->run(BACKWARD);
    motor4->run(BACKWARD);  
  for (speedSet = 0; speedSet < MAX_SPEED; speedSet + 2) // slowly bring the speed up to avoid loading down the batteries too quickly
  {
    motor1->setSpeed(speedSet);
    motor2->setSpeed(speedSet);
    motor3->setSpeed(speedSet);
    motor4->setSpeed(speedSet);
    delay(5);
  }
    }
}  

void turnRight() {
  motor1->setSpeed(100);
  motor2->setSpeed(100);
  motor3->setSpeed(100);
  motor4->setSpeed(100);
  
  motor1->run(FORWARD);
  motor2->run(FORWARD);
  motor3->run(BACKWARD);
  motor4->run(BACKWARD);     
  delay(500);
  motor1->run(FORWARD);      
  motor2->run(FORWARD);
  motor3->run(FORWARD);
  motor4->run(FORWARD);      
} 
 
void turnLeft() {
   motor1->setSpeed(100);
  motor2->setSpeed(100);
  motor3->setSpeed(100);
  motor4->setSpeed(100);
  
  motor1->run(BACKWARD);     
  motor2->run(BACKWARD);  
  motor3->run(FORWARD);
  motor4->run(FORWARD);   
  delay(500);
  motor1->run(FORWARD);     
  motor2->run(FORWARD);
  motor3->run(FORWARD);
  motor4->run(FORWARD);
}

I have attached the wiring diagram that was given for the project

speedSet + 2) Needs to be “+=”

The servo motors are actually moving rather than shifting slightly! Now the DC motors need to work, they work when I connect them with a regular batter or a breadboard so I don't think they're connected incorrectly, it may just be the program

    return distance;
    delay(100);

The delay will never get executed.

Progress! The servo motor turns right and left after sensing an object nearby and stays front when the front is clear. Since the sensors seem to be working and collecting distance data, would the code for moving forward and backward be incorrect since the DC motors are still unresponsive?

sapphirewalls:
Progress! The servo motor turns right and left after sensing an object nearby and stays front when the front is clear. Since the sensors seem to be working and collecting distance data, would the code for moving forward and backward be incorrect since the DC motors are still unresponsive?

Probably. Post your latest code with the fixes AWAOL pointed out.

The current code:

#include <Adafruit_MotorShield.h>  
#include <NewPing.h>
#include <Servo.h>

#define TRIG_PIN A0 
#define ECHO_PIN A1 
#define MAX_DISTANCE 200 
#define MAX_SPEED 190 // sets speed of DC  motors
#define MAX_SPEED_OFFSET 20

NewPing sonar(TRIG_PIN, ECHO_PIN, MAX_DISTANCE); 

Adafruit_MotorShield AFMS = Adafruit_MotorShield();

Adafruit_DCMotor *motor1 = AFMS.getMotor(1);
Adafruit_DCMotor *motor2 = AFMS.getMotor(2);
Adafruit_DCMotor *motor3 = AFMS.getMotor(3);
Adafruit_DCMotor *motor4 = AFMS.getMotor(4);
Servo myservo;   

boolean goesForward=false;
int distance = 100;
int speedSet = 0;

void setup() {

  AFMS.begin(1000);
  Serial.begin(9600);
  myservo.attach(10);  
  myservo.write(110); 
  delay(2000);
  distance = readPing();
  delay(100);
  distance = readPing();
  delay(100);
  distance = readPing();
  delay(100);
  distance = readPing();
  delay(100);
  
  
  
}

void loop() {
 int distanceR = 0;
 int distanceL =  0;
 delay(40);
 int distance = readPing();
 
 if(distance<=15)
 {
  moveStop();
  delay(100);
  moveBackward();
  delay(300);
  moveStop();
  delay(200);
  distanceR = lookRight();
  delay(200);
  distanceL = lookLeft();
  delay(200);

  if(distanceR>=distanceL)
  {
    turnRight();
    moveStop();
  }else
  {
    turnLeft();
    moveStop();
  }
 }else
 {
  moveForward();
 }
 
}

int lookRight()
{
    myservo.write(35); 
    delay(500);
    int distance = readPing();
    delay(100);
    myservo.write(110); 
    return distance;
    delay(100);
}

int lookLeft()
{
    myservo.write(175); 
    delay(500);
    int distance = readPing();
    delay(100);
    myservo.write(110); 
    return distance;
    delay(100);
}

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

void moveStop() {
  motor1->run(RELEASE); 
  motor2->run(RELEASE);
  motor3->run(RELEASE);
  motor4->run(RELEASE);
  } 
  
void moveForward() {

 if(goesForward==true)
  {
   
    motor1->run(FORWARD);      
    motor2->run(FORWARD);
    motor3->run(FORWARD); 
    motor4->run(FORWARD);     
   for (speedSet = 0; speedSet < MAX_SPEED; speedSet += 2) // slowly bring the speed up to avoid loading down the batteries too quickly
   {
    motor1->setSpeed(speedSet);
    motor2->setSpeed(speedSet);
    motor3->setSpeed(speedSet);
    motor4->setSpeed(speedSet);
    delay(5);
   }
  }
}

void moveBackward() {
    if(goesForward==false)
  {
    motor1->run(BACKWARD);      
    motor2->run(BACKWARD);
    motor3->run(BACKWARD);
    motor4->run(BACKWARD);  
  for (speedSet = 0; speedSet < MAX_SPEED; speedSet += 2) // slowly bring the speed up to avoid loading down the batteries too quickly
  {
    motor1->setSpeed(speedSet);
    motor2->setSpeed(speedSet);
    motor3->setSpeed(speedSet);
    motor4->setSpeed(speedSet);
    delay(5);
  }
    }
}  

void turnRight() {
  motor1->setSpeed(100);
  motor2->setSpeed(100);
  motor3->setSpeed(100);
  motor4->setSpeed(100);
  
  motor1->run(FORWARD);
  motor2->run(FORWARD);
  motor3->run(BACKWARD);
  motor4->run(BACKWARD);     
  delay(500);
  motor1->run(FORWARD);      
  motor2->run(FORWARD);
  motor3->run(FORWARD);
  motor4->run(FORWARD);      
} 
 
void turnLeft() {
   motor1->setSpeed(100);
  motor2->setSpeed(100);
  motor3->setSpeed(100);
  motor4->setSpeed(100);
  
  motor1->run(BACKWARD);     
  motor2->run(BACKWARD);  
  motor3->run(FORWARD);
  motor4->run(FORWARD);   
  delay(500);
  motor1->run(FORWARD);     
  motor2->run(FORWARD);
  motor3->run(FORWARD);
  motor4->run(FORWARD);
}

So you have a simple motor test sketch without the sonar and servo?

Is this a Servo library issue?

TheMemberFormerlyKnownAsAWOL:
So you have a simple motor test sketch without the sonar and servo?

Is this a Servo library issue?

I only tested the DC motors to see if they would work, but not together with a program. Do you suggest I upload a program that only moves the motors?
The Servo motor itself is working fine now and it seems the sensor is as well but the major issue I had with the original code given was that it used the Adafruit MotorShield V1 library and it wasn't cooperating so I tried my best to update it to the Adafruit MotorShield V2 library but I am unsure if I did it correctly.
I can try to make a simple motor test and upload it to see if the motors work...

When you are building a new system, writing individual sketches to test each hardware component is not something you should do late in the process. It should start at the beginning.

I found the problem and reversed most of my “updates” to the original code. For some reason when I uploaded the code it did not work at first so, while I was researching why I found someone say that the Adafruit MotorSheild V2 library worked better so I downloaded it and had to change the majority of the code. However, the Motor Shield I got was V1 and the library did not work. I now understand that I did not completely understand that they were mentioning the library and the motor shield itself. I am sorry for causing so much confusion but I appreciate all your help during this process. Since the original code works I didn’t really develop the code myself but I learned plenty through this whole process and I hope to be able to use the Arduino to develop sketches myself.

The original code is from DIY Builder on Youtube:

#include <AFMotor.h>  
#include <NewPing.h>
#include <Servo.h> 

#define TRIG_PIN A0 
#define ECHO_PIN A1 
#define MAX_DISTANCE 200 
#define MAX_SPEED 190 // sets speed of DC  motors
#define MAX_SPEED_OFFSET 20

NewPing sonar(TRIG_PIN, ECHO_PIN, MAX_DISTANCE); 

AF_DCMotor motor1(1, MOTOR12_1KHZ); 
AF_DCMotor motor2(2, MOTOR12_1KHZ);
AF_DCMotor motor3(3, MOTOR34_1KHZ);
AF_DCMotor motor4(4, MOTOR34_1KHZ);
Servo myservo;   

boolean goesForward=false;
int distance = 100;
int speedSet = 0;

void setup() {

  myservo.attach(10);  
  myservo.write(115); 
  delay(2000);
  distance = readPing();
  delay(100);
  distance = readPing();
  delay(100);
  distance = readPing();
  delay(100);
  distance = readPing();
  delay(100);
}

void loop() {
 int distanceR = 0;
 int distanceL =  0;
 delay(40);
 
 if(distance<=15)
 {
  moveStop();
  delay(100);
  moveBackward();
  delay(300);
  moveStop();
  delay(200);
  distanceR = lookRight();
  delay(200);
  distanceL = lookLeft();
  delay(200);

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

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

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

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

void moveStop() {
  motor1.run(RELEASE); 
  motor2.run(RELEASE);
  motor3.run(RELEASE);
  motor4.run(RELEASE);
  } 
  
void moveForward() {

 if(!goesForward)
  {
    goesForward=true;
    motor1.run(FORWARD);      
    motor2.run(FORWARD);
    motor3.run(FORWARD); 
    motor4.run(FORWARD);     
   for (speedSet = 0; speedSet < MAX_SPEED; speedSet +=2) // slowly bring the speed up to avoid loading down the batteries too quickly
   {
    motor1.setSpeed(speedSet);
    motor2.setSpeed(speedSet);
    motor3.setSpeed(speedSet);
    motor4.setSpeed(speedSet);
    delay(5);
   }
  }
}

void moveBackward() {
    goesForward=false;
    motor1.run(BACKWARD);      
    motor2.run(BACKWARD);
    motor3.run(BACKWARD);
    motor4.run(BACKWARD);  
  for (speedSet = 0; speedSet < MAX_SPEED; speedSet +=2) // slowly bring the speed up to avoid loading down the batteries too quickly
  {
    motor1.setSpeed(speedSet);
    motor2.setSpeed(speedSet);
    motor3.setSpeed(speedSet);
    motor4.setSpeed(speedSet);
    delay(5);
  }
}  

void turnRight() {
  motor1.run(FORWARD);
  motor2.run(FORWARD);
  motor3.run(BACKWARD);
  motor4.run(BACKWARD);     
  delay(500);
  motor1.run(FORWARD);      
  motor2.run(FORWARD);
  motor3.run(FORWARD);
  motor4.run(FORWARD);      
} 
 
void turnLeft() {
  motor1.run(BACKWARD);     
  motor2.run(BACKWARD);  
  motor3.run(FORWARD);
  motor4.run(FORWARD);   
  delay(500);
  motor1.run(FORWARD);     
  motor2.run(FORWARD);
  motor3.run(FORWARD);
  motor4.run(FORWARD);
}

Thank you so much again :slight_smile: