Details of the maze robotic car

fridayabraham · November 17, 2018, 1:17am

Good morning
#define TRIGGER_PINL A3 // Arduino pin tied to trigger pin on ping sensor.
#define ECHO_PINL A0 // Arduino pin tied to echo pin on ping sensor.

#define MAX_DISTANCE 100 // Maximum distance we want to ping for (in centimeters). Maximum sensor distance is rated at 400-500cm.

#define TRIGGER_PINF A4 // Arduino pin tied to trigger pin on ping sensor.
#define ECHO_PINF A1 // Arduino pin tied to echo pin on ping sensor.

#define TRIGGER_PINR A5 // Arduino pin tied to trigger pin on ping sensor.
#define ECHO_PINR A2 // Arduino pin tied to echo pin on ping sensor.

int dir;

#define STOP 0
#define FORWARD 1
#define BACKWARD 2
#define LEFT 3
#define RIGHT 4

float P = 0.7 ;
float D = 0.5 ;
float I = 0.4 ;
float oldErrorP ;
float totalError ;
int offset = 5 ;

int wall_threshold = 13 ;
//int left_threshold = 10 ;
//int right_threshold = 10 ;
int front_threshold = 7 ;

boolean frontwall ;
boolean leftwall ;
boolean rightwall ;
boolean first_turn ;
boolean rightWallFollow ;
boolean leftWallFollow ;

int en1 = 2 ;
int en2 = 3 ;

int en3 = 4 ;
int en4 = 5 ;

int enA = 10 ;
int enB = 11 ;

int baseSpeed = 70 ;

int RMS ;
int LMS ;

int LED = 13 ;
int led1 = 8 ;
int led2 = 9 ;

NewPing sonarLeft(TRIGGER_PINL, ECHO_PINL, MAX_DISTANCE); // NewPing setup of pins and maximum distance.
NewPing sonarRight(TRIGGER_PINR, ECHO_PINR, MAX_DISTANCE);
NewPing sonarFront(TRIGGER_PINF, ECHO_PINF, MAX_DISTANCE);

unsigned int pingSpeed = 30; // How frequently are we going to send out a ping (in milliseconds). 50ms would be 20 times a second.
unsigned long pingTimer; // Holds the next ping time.

float oldLeftSensor, oldRightSensor, leftSensor, rightSensor, frontSensor, oldFrontSensor, lSensor, rSensor, fSensor;

//int TestNUM = 1 ;

void setup() {

Serial.begin(115200); // Open serial monitor at 115200 baud to see ping results.

for (int i = 2; i <= 13; i++) //For Motor Shield
pinMode(i, OUTPUT);

first_turn = false ;
rightWallFollow = false ;
leftWallFollow = false ;

}

void loop() {

//========================================START========================================//

ReadSensors();

walls();

if ( first_turn == false ) {

pid_start();

}
else if (leftWallFollow == true ) {

PID(true) ;

}
else if (rightWallFollow == true ) {
PID(false) ;
}

if (leftwall == true && rightwall == false && frontwall == true ) {

// turnright();
PID(false) ;

if ( first_turn == false ) {

  //      right_threshold = right_threshold - offset ;
  //      left_threshold = left_threshold + offset ;


  first_turn = true ;
  rightWallFollow = true ;
  
  digitalWrite(led2 , LOW );
  digitalWrite(led1 ,HIGH );
}

}
if (leftwall == false && rightwall == true && frontwall == true ) {

//  turnleft();
PID(true) ;

if ( first_turn == false ) {

  //      left_threshold = left_threshold - offset ;
  //      right_threshold = right_threshold + offset ;

  first_turn = true ;
  leftWallFollow = true ;
  digitalWrite(LED , HIGH);
   
}

}
if ( leftSensor == 0 || leftSensor > 100 && rightSensor == 0 || rightSensor > 100 && frontSensor == 0 || frontSensor > 100 ) {

setDirection(STOP);

}

// read sensors & print the result to the serial monitor //

Serial.print(" Left : “);
Serial.print(leftSensor);
Serial.print(” cm “);
Serial.print(” Right : “);
Serial.print(rightSensor);
Serial.print(” cm “);
Serial.print(” Front : “);
Serial.print(frontSensor);
Serial.println(” cm ");

//measure error & print the result to the serial monitor

Serial.print(“error=”);
Serial.println(totalError);

}

//--------------------------------- direction control ---------------------------------//

void setDirection(int dir) {

if ( dir == FORWARD ) {
digitalWrite(en1, LOW); // Left wheel forward
digitalWrite(en2, HIGH);
digitalWrite(en3, LOW); // Right wheel forward
digitalWrite(en4, HIGH);
}
else if ( dir == LEFT ) {
digitalWrite(en1, HIGH); // Left wheel forward
digitalWrite(en2, LOW );
digitalWrite(en3, LOW ); // Right wheel forward
digitalWrite(en4, HIGH);
}
else if ( dir == RIGHT ) {
digitalWrite(en1, LOW); // Left wheel forward
digitalWrite(en2, HIGH);
digitalWrite(en3, HIGH); // Right wheel forward
digitalWrite(en4, LOW);
}
else if ( dir == STOP ) {
digitalWrite(en1, HIGH); // Left wheel forward
digitalWrite(en2, HIGH );
digitalWrite(en3, HIGH ); // Right wheel forward
digitalWrite(en4, HIGH);
}
else if ( dir == BACKWARD ) {
digitalWrite(en1, HIGH ); // Left wheel forward
digitalWrite(en2, LOW );
digitalWrite(en3, HIGH ); // Right wheel forward
digitalWrite(en4, LOW );
}
}
//---------------------------------------------------------------------------//

//--------------------------------- Sensors ---------------------------------//

void ReadSensors() {

//leftSensor = sonarLeft.ping_median(TestNUM); //accurate but slow
//rightSensor = sonarRight.ping_median(TestNUM); //accurate but slow
//frontSensor = sonarFront.ping_median(TestNUM); //accurate but slow

//leftSensor = sonarLeft.convert_cm(leftSensor);
//rightSensor = sonarRight.convert_cm(rightSensor);
//frontSensor = sonarFront.convert_cm(frontSensor);

lSensor = sonarLeft.ping_cm(); //ping in cm
rSensor = sonarRight.ping_cm();
fSensor = sonarFront.ping_cm();

leftSensor = (lSensor + oldLeftSensor) / 2; //average distance between old & new readings to make the change smoother
rightSensor = (rSensor + oldRightSensor) / 2;
frontSensor = (fSensor + oldFrontSensor) / 2;

oldLeftSensor = leftSensor; // save old readings for movment
oldRightSensor = rightSensor;
oldFrontSensor = frontSensor;

}

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

//--------------------------------- control ---------------------------------//

void pid_start() {

//ReadSensors()

float errorP = leftSensor - rightSensor ;
float errorD = errorP - oldErrorP;
float errorI = (2.0 / 3.0) * errorI + errorP ;

totalError = P * errorP + D * errorD + I * errorI ;

oldErrorP = errorP ;

RMS = baseSpeed + totalError ;
LMS = baseSpeed - totalError ;

// if(RMS < -255) RMS = -255; if(RMS > 255)RMS = 255 ;
// if(LMS < -255) LMS = -255; if(LMS > 255)LMS = 255 ;

if (RMS < 0) {

RMS = map(RMS , 0 , -255, 0, 255);

analogWrite(enA , RMS);
analogWrite(enB , LMS);

setDirection(RIGHT);

}
else if (LMS < 0) {
LMS = map(LMS , 0 , -255, 0, 255);

analogWrite(enA , RMS);
analogWrite(enB , LMS);

setDirection(LEFT);

}
else {

analogWrite(enA , RMS);
analogWrite(enB , LMS);

setDirection(FORWARD);

}

//----------------------------- wall follow control -------------------------------//

void PID( boolean left ) {

if (left == true ) {

float errorP = leftSensor - rightSensor - offset ;
float errorD = errorP - oldErrorP;
float errorI = (2.0 / 3) * errorI + errorP ;


totalError = P * errorP + D * errorD + I * errorI ;

oldErrorP = errorP ;


RMS = baseSpeed + totalError ;
LMS = baseSpeed - totalError ;

//  if(RMS < -255) RMS = -255; if(RMS > 255)RMS = 255 ;
//  if(LMS < -255) LMS = -255;  if(LMS > 255)LMS = 255 ;


if (RMS < 0) {

  RMS = map(RMS , 0 , -255, 0, 255);

  analogWrite(enA , RMS);
  analogWrite(enB , LMS);

  setDirection(RIGHT);

}
else if (LMS < 0) {
  LMS = map(LMS , 0 , -255, 0, 255);


  analogWrite(enA , RMS);
  analogWrite(enB , LMS);

  setDirection(LEFT);
}
else {

  analogWrite(enA , RMS);
  analogWrite(enB , LMS);

  setDirection(FORWARD);
}

}
else {

float errorP = leftSensor - rightSensor + offset ;
float errorD = errorP - oldErrorP;
float errorI = (2.0 / 3) * errorI + errorP ;

totalError = P * errorP + D * errorD + I * errorI ;

oldErrorP = errorP ;


RMS = baseSpeed + totalError ;
LMS = baseSpeed - totalError ;

//  if(RMS < -255) RMS = -255; if(RMS > 255)RMS = 255 ;
//  if(LMS < -255) LMS = -255;  if(LMS > 255)LMS = 255 ;


if (RMS < 0) {

  RMS = map(RMS , 0 , -255, 0, 255);

  analogWrite(enA , RMS);
  analogWrite(enB , LMS);

  setDirection(RIGHT);

}
else if (LMS < 0) {
  LMS = map(LMS , 0 , -255, 0, 255);


  analogWrite(enA , RMS);
  analogWrite(enB , LMS);

  setDirection(LEFT);
}
else {

  analogWrite(enA , RMS);
  analogWrite(enB , LMS);

  setDirection(FORWARD);
}

}

//--------------------------- wall detection --------------------------------//

void walls() {

if ( leftSensor < wall_threshold ) {
leftwall = true ;
}
else {
leftwall = false ;
}

if ( rightSensor < wall_threshold ) {
rightwall = true ;
}
else {
rightwall = false ;

} if ( frontSensor < front_threshold ) {
frontwall = true ;
}
else {
frontwall = false ;
}

}

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

void turnright() {

LMS = baseSpeed ;

RMS = LMS * rightSensor / ( rightSensor + 11 ) ;

}

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

void turnleft() {

RMS = baseSpeed ;

LMS = RMS * leftSensor / ( leftSensor + 11 ) ;

}
I want the maze to be able to move out of any maze using wall following agorithm and also with the help of the Ultrasonic sensors. am trying to combine the movement from left and right fall following agorithm and also codes from the Ultrasonic sensors for better decision making. I don’t really know how to use color sensor especially in this situation.

Am working with 3 ultrasonic sensors

1 motor controller(h- bridge L298N)
1 colour sensor
2 actuators ( motors)
Using 3 LED’s for design

The problem am having is that my ultrasonic sensors are not working according to the code so as my motors

cbenson · November 26, 2018, 1:23pm

Did you create this code yourself, or are you using someone else’s? It’s quite a bit of code to troubleshoot, so it’s best that you do it in sections - get the ultrasonic sensors up and running on their own first.

fridayabraham · November 26, 2018, 10:23pm

Cbenson I created the code my self but the robot is not working as planned

fridayabraham · November 26, 2018, 11:15pm

This is the code am currently using I created this myself the one you saw was copied from instructables.
The problem now is that only two of the three ultrasonic sensors (front and left US sensors)are working inline with the motors right US sensor is not.
Please cbenson I have to complete and resolve this before the competition.

#define leftVel 10
#define rightVel 5
#define In1 9
#define In2 8
#define In3 7
#define In4 6
int trigger_front = A4;
int echo_front = A5; // front
int trigger_right = A2;// right
int echo_right = A3;// rt
int trigger_left = A0;
int echo_left = A1;
int delayTime;
int collision = 20;
long int Fdistance, Ldistance, Rdistance, Ftime, Ltime, Rtime;

void setup() {
// put your setup code here, to run once:
pinMode(leftVel, OUTPUT);
pinMode(rightVel, OUTPUT);
pinMode(In1, OUTPUT);
pinMode(In2, OUTPUT);
pinMode(In3, OUTPUT);
pinMode(In4, OUTPUT);
pinMode(trigger_front, OUTPUT);
pinMode(echo_front, INPUT);
pinMode(trigger_right, OUTPUT);
pinMode(echo_right, INPUT);
pinMode(trigger_left, OUTPUT);
pinMode(echo_left, INPUT);

}
//-----------------------------------------------------------------------------
// FUNCTION TO MOVE FOWARD
//-----------------------------------------------------------------------------
void stright()
{
digitalWrite(In1, HIGH);
digitalWrite(In2, LOW);
analogWrite(leftVel, 200);
digitalWrite(In3, HIGH);
digitalWrite(In4, LOW);
analogWrite(rightVel, 200);
}
//-----------------------------------------------------------------------------
// FUNCTION TO GO LEFT
//-----------------------------------------------------------------------------

void left()
{
digitalWrite(In1, HIGH);
digitalWrite(In2, LOW);
analogWrite(leftVel, 200);
digitalWrite(In3, LOW);
digitalWrite(In4, HIGH);
analogWrite(rightVel, 200);
}
//-----------------------------------------------------------------------------
// FUNCTION TO GO RIGHT
//-----------------------------------------------------------------------------
void right()
{
digitalWrite(In1, LOW);
digitalWrite(In2, HIGH);
analogWrite(leftVel, 200);
digitalWrite(In3, HIGH);
digitalWrite(In4, LOW);
analogWrite(rightVel, 200);
}
//-----------------------------------------------------------------------------
// FUNCTION TO STOP
//-----------------------------------------------------------------------------
void halt()
{
digitalWrite(In1, LOW);
digitalWrite(In2, LOW);
analogWrite(leftVel, 0);
digitalWrite(In3, LOW);
digitalWrite(In4, LOW);
analogWrite(rightVel, 0);
}
//-----------------------------------------------------------------------------
// FUNCTION TO TURN VEHICLES AROUND
//-----------------------------------------------------------------------------
void turnAround()
{
left();
delay(100);
}

//-----------------------------------------------------------------------------
// FUNCTION TO COMPARE DISTANCES AND TAKE NECESSARY ACTION
//-----------------------------------------------------------------------------
void compareDistance() // find the longest distance
{
if (Ldistance>Rdistance) //if left is less obstructed
{
left();
}
else if (Rdistance>Ldistance) //if right is less obstructed
{
right();
}
else //if they are equally obstructed
{
turnAround();
}
}

void changeDirection ()
{

halt();
compareDistance();

}

void loop() {
// put your main code here, to run repeatedly:
//-----------------------------------------------------------------------------
//Step 1 - //to calculate the respective distances to the front left and right
//-----------------------------------------------------------------------------

        digitalWrite(trigger_front, LOW);
        delayMicroseconds(2);
        digitalWrite(trigger_front, HIGH);
        delayMicroseconds(5);
        digitalWrite(trigger_front, LOW);
        Ftime = pulseIn(echo_front, HIGH);
        Fdistance = Ftime/29/2;
        digitalWrite(trigger_right, LOW);
        delayMicroseconds(2);
        digitalWrite(trigger_right, HIGH);
        delayMicroseconds(5);
        digitalWrite(trigger_right, LOW);
        Rtime = pulseIn(echo_right, HIGH);
        Rdistance = Rtime/29/2;
        digitalWrite(trigger_left, LOW);
        delayMicroseconds(2);
        digitalWrite(trigger_left, HIGH);
        delayMicroseconds(5);
        digitalWrite(trigger_left, LOW);
        Ltime = pulseIn(echo_left, HIGH);
        Ldistance = Ltime/29/2;

//-----------------------------------------------------------------------------
//Step 2 - decesions
//-----------------------------------------------------------------------------

if (Fdistance <= collision) {changeDirection();}
else stright();
delay(1000);
}

oldguy · November 26, 2018, 11:36pm

You are making it as hard as possible to help you.
Several people have asked for information from you so that we could help. You refuse to provide it.
In the time I have spend simply trying to get you to give us that information I could have written the code from scratch.
You continue to waste people’s time making multiple posts for the same problem, asking us to fix things that aren’t broken, asking us to debug code that you are no longer using, …
At some point even the most patient and helpful person will give up. I’m not the most patient and helpful person. I have reached my limit already.
But perhaps what I have said can convince you to provide the information people need to help you. And just maybe someone else will come along and do that. It just won’t be me.

fridayabraham · November 26, 2018, 11:47pm

Am sorry for the delay most times am too busy to check my chat

fridayabraham · November 26, 2018, 11:52pm

Am new to this platform that’s the reason why I’ve not been going through my messages. please accept my apologies.

fridayabraham · November 27, 2018, 10:20am

The robotic car is meant to navigate it way out of a maze with the help of the the three ultrasonic sensors (front, left and right US sensors).
Once again am sorry for putting you through all the stress.

1358437310965969 · January 7, 2019, 1:54am

Hei !!! I’m very desparetley looking for any maze solver( made with walls) code. I have ultrasonic sensors and ir sensors and arduino. I’m only able to make a open loop control system not the closed loop control system.

And is that code working.?@fridayabraham

cbenson · January 7, 2019, 1:21pm

Note that any code is hardware specific - it would likely need to be the same (or directly compatible) microcontroller, sensors, motor controller etc. What you might instead look for is pseudo code which you can adapt to your specific setup. Take a look at any maze solving (walls or line following) kit, and there’s usually sample code provided.