Obstacle avoiding robot car -- code issue

Hello,
I have been building an obstacle avoiding robot car with ultrasonic sensor and a 4 wheel motor driver. I have used this motor sheild . Now I'm struggling with the code. Please can anyone help me with this problem. Here is the code I am using

Summary

This text will be hidden

[ code ] // obstacle robot code

#include <AFMotor.h> //add Adafruit Motor Shield library
#include <Servo.h> //add Servo Motor library
#include <NewPing.h> //add Ultrasonic sensor library

#define TRIG_PIN A0 // Pin A0 on the Motor Drive Shield soldered to the ultrasonic sensor
#define ECHO_PIN A1 // Pin A1 on the Motor Drive Shield soldered to the ultrasonic sensor
#define MAX_DISTANCE 200 // sets maximum useable sensor measuring distance to 300cm
#define MAX_SPEED 160 // sets speed of DC traction motors to 150/250 or about 70% of full speed - to get power drain down.
#define MAX_SPEED_OFFSET 40 // this sets offset to allow for differences between the two DC traction motors
#define COLL_DIST 20 // sets distance at which robot stops and reverses to 30cm
#define TURN_DIST COLL_DIST+20 // sets distance at which robot veers away from object
NewPing sonar(TRIG_PIN, ECHO_PIN, MAX_DISTANCE); // sets up sensor library to use the correct pins to measure distance.

AF_DCMotor leftMotor1(1, MOTOR12_1KHZ); // create motor #1 using M1 output on Motor Drive Shield, set to 1kHz PWM frequency
AF_DCMotor leftMotor2(2, MOTOR12_1KHZ); // create motor #2, using M2 output, set to 1kHz PWM frequency
AF_DCMotor rightMotor1(3, MOTOR34_1KHZ);// create motor #3, using M3 output, set to 1kHz PWM frequency
AF_DCMotor rightMotor2(4, MOTOR34_1KHZ);// create motor #4, using M4 output, set to 1kHz PWM frequency
Servo myservo; // create servo object to control a servo

int leftDistance, rightDistance; //distances on either side
int curDist = 0;
String motorSet = "";
int speedSet = 0;
int servoFront = 115;
int servoLeft = 170;
int servoRight = 50;

//------------------------------------------ SETUP_LOOP --------------------------------------------

void setup() {
Serial.begin(9600);
myservo.attach(10); // attaches the servo on pin 10 (SERVO_1 on the Motor Drive Shield to the servo object
myservo.write(servoFront); // tells the servo to position at 115--degrees ie. facing forward.
delay(1000); // delay for one seconds
}
//------------------------------------------------------------------------------------------------------------------------------------

//--------------------------------------MAIN_LOOP---------------------------------------------------

void loop() {
myservo.write(servoFront); // move eyes forward
curDist = readPing(); // read distance
if (curDist < COLL_DIST) {changePath();} // if forward is blocked change direction
moveForward(); // move forward

}

//--------------------------------------------------------------------------------------------------

void changePath() {
moveStop(); // stop forward movement
myservo.write(servoRight); // check distance to the right
delay(800);
rightDistance = readPing(); //set right distance
delay(800);
myservo.write(servoLeft); // check distace to the left
delay(800);
leftDistance = readPing(); //set left distance
delay(800);
myservo.write(servoFront); //returns the servo to center
delay(1000);
compareDistance();
}

void compareDistance() // find the longest distance
{
if (leftDistance>rightDistance) //if left is less obstructed
{
turnLeft();
}
else if (rightDistance>leftDistance) //if right is less obstructed
{
turnRight();
}
else if (rightDistance=leftDistance) // if both are eqall
{
turnAround();
}
}

//-------------------------------------------------------------------------------------------------------------------------------------

int readPing() { // read the ultrasonic sensor distance
delay(70);
unsigned int uS = sonar.ping();
int cm = uS/US_ROUNDTRIP_CM;
return cm;
}
//-------------------------------------------------------------------------------------------------------------------------------------
void moveStop() {
leftMotor1.run(RELEASE);
leftMotor2.run(RELEASE);
rightMotor1.run(RELEASE);
rightMotor2.run(RELEASE);} // stop the motors.
//-------------------------------------------------------------------------------------------------------------------------------------
void moveForward() {
motorSet = "FORWARD";
leftMotor1.run(FORWARD); // turn it on going forward
leftMotor2.run(FORWARD); // turn it on going forward
rightMotor1.run(FORWARD); // turn it on going forward
rightMotor2.run(FORWARD); // turn it on going forward
for (speedSet = 0; speedSet < MAX_SPEED; speedSet +=2) // slowly bring the speed up to avoid loading down the batteries too quickly
{
leftMotor1.setSpeed(speedSet);
leftMotor2.setSpeed(speedSet);
rightMotor1.setSpeed(speedSet);
rightMotor2.setSpeed(speedSet);
delay(5);
}
}
//-------------------------------------------------------------------------------------------------------------------------------------
void moveBackward() {
motorSet = "BACKWARD";
leftMotor1.run(BACKWARD); // turn it on going backward
leftMotor2.run(BACKWARD); // turn it on going backward
rightMotor1.run(BACKWARD); // turn it on going backward
rightMotor2.run(BACKWARD); // turn it on going backward
for (speedSet = 0; speedSet < MAX_SPEED; speedSet +=2) // slowly bring the speed up to avoid loading down the batteries too quickly
{
leftMotor1.setSpeed(speedSet);
leftMotor2.setSpeed(speedSet);
rightMotor1.setSpeed(speedSet);
rightMotor2.setSpeed(speedSet);
delay(5);
}
}
//-------------------------------------------------------------------------------------------------------------------------------------
void turnRight() {
motorSet = "RIGHT";
leftMotor1.run(FORWARD); // turn motor 1 forward
leftMotor2.run(FORWARD); // turn motor 2 forward
rightMotor1.run(BACKWARD); // turn motor 3 backward
rightMotor2.run(BACKWARD); // turn motor 4 backward
rightMotor1.setSpeed(speedSet+MAX_SPEED_OFFSET);
rightMotor2.setSpeed(speedSet+MAX_SPEED_OFFSET);
delay(1500); // run motors this way for 1500
motorSet = "FORWARD";
leftMotor1.run(FORWARD); // set both motors back to forward
leftMotor2.run(FORWARD);
rightMotor1.run(FORWARD);
rightMotor2.run(FORWARD);
}
//-------------------------------------------------------------------------------------------------------------------------------------
void turnLeft() {
motorSet = "LEFT";
leftMotor1.run(BACKWARD); // turn motor 1 backward
leftMotor2.run(BACKWARD); // turn motor 2 backward
leftMotor1.setSpeed(speedSet+MAX_SPEED_OFFSET);
leftMotor2.setSpeed(speedSet+MAX_SPEED_OFFSET);
rightMotor1.run(FORWARD); // turn motor 3 forward
rightMotor2.run(FORWARD); // turn motor 4 forward
delay(1500); // run motors this way for 1500
motorSet = "FORWARD";
leftMotor1.run(FORWARD); // turn it on going forward
leftMotor2.run(FORWARD); // turn it on going forward
rightMotor1.run(FORWARD); // turn it on going forward
rightMotor2.run(FORWARD); // turn it on going forward
}
//-------------------------------------------------------------------------------------------------------------------------------------
void turnAround() {
motorSet = "RIGHT";
leftMotor1.run(FORWARD); // turn motor 1 forward
leftMotor2.run(FORWARD); // turn motor 2 forward
rightMotor1.run(BACKWARD); // turn motor 3 backward
rightMotor2.run(BACKWARD); // turn motor 4 backward
rightMotor1.setSpeed(speedSet+MAX_SPEED_OFFSET);
rightMotor2.setSpeed(speedSet+MAX_SPEED_OFFSET);
delay(1700); // run motors this way for 1700
motorSet = "FORWARD";
leftMotor1.run(FORWARD); // set both motors back to forward
leftMotor2.run(FORWARD);
rightMotor1.run(FORWARD);
rightMotor2.run(FORWARD);
}
[ /code ]
.
Please if anybody have the correct code send it to me, as fast as you can, as a reply on this post or you can e-mail me on tarshgautam@yahoo.com .

THANK YOU

What is the problem?

Please remember to use code tags when posting code

Hello TheMemberFormerlyKnownAsAWOL,
The problem is that when I burn the code into my arduino UNO, my servo motor starts moving and the wheels go forward and backward for a couple of seconds. And this thing repeat like a cycle again and again. If you have made any obstacle avoiding car or robot, Please copy and send the code to my email address
Sorry, I forgot to add a code tag.

EMAIL ADRESS REMOVED BY MODERATOR.

No problem. We can wait.

Can you read 6809 assembler?

Edit: scratch that: the source is on paper tape.

I noticed you have a Serial begin but I can't see many serial debug prints.
Fix that.

Hi, @tarsh
Welcome to the forum.

Please read the post at the start of any forum , entitled "How to use this Forum".

Have you got ANY code that just controls the wheels, NOTHING else in the code?

Have you developed the code in stages?
Have you got code that JUST controls, ONE motor?
Have you got code that JUST controls BOTH motors?
Have you got code that JUST controls the SERVO?
Have you got code that JUST reads the UltraSonic Sensor?

If not, can I suggest you start there and get each bit of code working, then add them together one at a time and make sure they work before adding the bext.

This will simplify the debugging if you have no errors in the different parts.

Thanks.. Tom... :smiley: :+1: :coffee: :australia:

Too easy. CDP1802 I say.

Z80 here..

About 30 years ago, that was my "bread and butter"! :sunglasses:

Me too, but closer to forty years for me, but I was mistaken - my code is for a 6802.
Unfortunately, I've lost the schematic, so the code wouldn't be much use anyway.

My bread and butter 30++ years ago plus hobby during 15 years..

I once managed to write code for a traffic light sequence on a Nascom 1 (Z80)… you had to code in hex for that

1 Like

Nascom 1 was a brilliant machine, wasn't it?

I should perhaps clarify - it wasn't my day job as such. (Too bad!) :grin:

That was hard work, coding in hex, machine code.
The computer friends at the university disassembled Cromemco Rose which made things a lot easier... An assembler, compiler.... Typing the entire sorcecode, storing it on tape(!), test compiling and correcting mistypings...
Those were the days...

1 Like