Wall follower with adafruit motorshield and HC-SR04

Hello, Im new in here, but Im realy strugling with this project. Im writing code with foxytronics example, eveything looks good but my motors wont start. Firstly Im trying to do is run this with 2 DC mottors, later on I`ll upgrade it to 4DC motors and 2 sensors for better results. Serials monitor shows speed, I set it to run mottors, but nothing happens. Please any help would be appreciate. Sorry for my english, might be broken :smiley: Thank you in advance!

My code is:

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

// create servo objects
AF_DCMotor Motor1(1, MOTOR12_1KHZ); // create motor #1, 1KHz pwm
AF_DCMotor Motor2(2, MOTOR12_1KHZ); // create motor #2, 1KHz pwm


int speed_min = 135; //the minimum "speed" the motors will turn - take it lower and motors don't turn
int speed_max = 255; //the maximum "speed" the motors will turn ‚Äď you can‚Äôt put in higher

const int serialPeriod = 250;       // only print to the serial console every 1/4 second
unsigned long timeSerialDelay = 0;

const int loopPeriod = 100;         // a period of 100ms = a frequency of 10Hz
unsigned long timeLoopDelay   = 0;

// specify the trig & echo pins used for the ultrasonic sensors
const int ultrasonic2TrigPin = A0;
const int utlrasonic2EchoPin = A1;

int ultrasonic2Distance;
int ultrasonic2Duration;

// PID variables
const int targetDistance = 152; // target distance from wall is 6" (~152mm)
int e; // the error (target distance - measured distance)
int u; // the PID output
int p = 0;
float i = 0;

// PID gains
const float K_p = 1;
const float K_i = 0.0075;

const int motorNeutral = 255;
const int baseMotorSpeed = 200; // 90 is full speed (once added or subtracted from motorNeutral)



int speed1 = speed_max; 
int speed2 = speed_max;

void setup()
{
   Serial.begin(9600);
 
   // ultrasonic sensor pin configurations
   pinMode(ultrasonic2TrigPin, OUTPUT);
   pinMode(utlrasonic2EchoPin, INPUT);
 

}


void loop()
{
   debugOutput(); // prints debugging messages to the serial console
   
   if(millis() - timeLoopDelay >= loopPeriod)
   {
       readUltrasonicSensors(); // read and store the measured distances
   
       followWall();
       
       timeLoopDelay = millis();
   }

   Motor1.setSpeed(speed1);
   Motor2.setSpeed(speed2);
}


void readUltrasonicSensors()
{
   // ultrasonic 2
   digitalWrite(ultrasonic2TrigPin, HIGH);
   delayMicroseconds(10);                  // must keep the trig pin high for at least 10us
   digitalWrite(ultrasonic2TrigPin, LOW);
   
   ultrasonic2Duration = pulseIn(utlrasonic2EchoPin, HIGH);
   ultrasonic2Distance = (ultrasonic2Duration/2)/3;
}


void followWall()
{
   int oldE = e;
   
   e = targetDistance - ultrasonic2Distance;
   
   p = K_p*e; // p gets bigger when the wall is too close and smaller when it is too far away
   i += K_i*e; // i gets bigger when the wall is too close and smaller when it is too far away
   
//    // limit p
   if(p > 50)
    p = 50;
   else if(p < -50)
    p = -50;
   
   // limit i
   if(i > 50)
     i = 50;
   else if(i < -50)
     i = -50;
     
   u = p + i;
   
   speed1  = motorNeutral - baseMotorSpeed - u;
   speed2 = motorNeutral + baseMotorSpeed - u;
   
   // limit leftMotorSpeed
   if(speed1 > 255)
     speed1 = 255;
   else if(speed1 < 0)
     speed1 = 0;
     
   // limit rightMotorSpeed
   if(speed2 > 255)
     speed2 = 255;
   else if(speed2 < 0)
     speed2 = 0;
   
  
}


void debugOutput()
{
   if((millis() - timeSerialDelay) > serialPeriod)
   {
//        Serial.print("ultrasonic2Distance: ");
//        Serial.print(ultrasonic2Distance);
//        Serial.print("mm");
//        Serial.println();
       
       Serial.print("speed1: ");
       Serial.print(speed1);
       Serial.println();
       
       Serial.print("speed2: ");
       Serial.print(speed2);
       Serial.println();
       
//        Serial.print("e: ");
//        Serial.print(e);
//        Serial.println();
//
       Serial.print("p: ");
       Serial.print(p);
       Serial.println();
       
       Serial.print("i: ");
       Serial.print(i);
       Serial.println();
       
//        Serial.print("u: ");
//        Serial.print(u);
//        Serial.println();
       
       Serial.println();
       
       timeSerialDelay = millis();
   }
}

I`m new in here,

Then you will have read read this before posting a programming question so will know how to post code to make it easier to read and copy.

How are the motors powered ? Please post a diagram of your circuit.

I`ll try to improve so it will be easier for you to understand me.

Everything is connected with arduino uno, adafruit motor shield, 4 DC motors (now testing 2), and 1 HC-SR04 sensor (will try to make with 2-3). All powered directly with arduino uno and 4AA batteries.

I dont have a sketch yet, but Ill upload a photo so you could get at least a small understanding, how everyting works. I tried it already with basic obstackle avoidance, everything works fine. Just can`t get it to follow a wall :smiley: Thanks!

Look at what your code does. Every 100 milliseconds, open your eyes (read the ping sensor) and determine the distance to the nearest obstacle (in a strange way). Then, determine what the speeds should be, based on the distance to the obstacle. Then, on every pass through loop(), set the speed of the motors.

Why? You should be looking at how far you are from the wall as often as you can. You only need to set the speeds if the distance to the wall changes.

Sorry for that, I was just trying to figure out how code works, because examples of it in internet, works better. Now I`m trying to make everything by my own from beginning. I was wondering, is there so kind of way to calculate corner size with HC-SR04. I would like robot to stop spinning at the corner if he reaches 90 degrees with wall. Thanks!

My code so far: (PS. Some comments and serial writes are in my language. Just ignore them :smiley: )

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


//Sukuriame motor shields

AF_DCMotor motor1(1, MOTOR12_1KHZ); // create motor #1, 1KHz pwm
AF_DCMotor motor2(2, MOTOR12_1KHZ); // create motor #2, 1KHz pwm
AF_DCMotor motor3(3, MOTOR12_1KHZ); // create motor #1, 1KHz pwm
AF_DCMotor motor4(4, MOTOR12_1KHZ); // create motor #2, 1KHz pwm

int RangeFront = 0;
int RangeWall = 0;

int WayTooClose = 30;
int EndOfWall = 50; 

//Nustatomas numatomas motoru greitis

int speed_min = 135; //the minimum "speed" the motors will turn - take it lower and motors don't turn
int speed_max = 255; //the maximum "speed" the motors will turn ‚Äď you can‚Äôt put in higher

int speed1 = speed_max; // set both motors to maximum speed
int speed2 = speed_max;
int speed3 = speed_max; // set both motors to maximum speed
int speed4 = speed_max;

//Sensoriai

#define MAX_DISTANCE 800 // Maximum distance to ping. Edit the distance as needed.

const int ultrasonic2TrigPin1 = A0;
const int utlrasonic2EchoPin1 = A1;
const int ultrasonic2TrigPin2 = A2;
const int utlrasonic2EchoPin2 = A3;

NewPing DistanceSensor1(ultrasonic2TrigPin1, utlrasonic2EchoPin1, MAX_DISTANCE);
NewPing DistanceSensor2(ultrasonic2TrigPin2, utlrasonic2EchoPin2, MAX_DISTANCE);


void setup()
{
    Serial.begin(9600);
 
}

void loop()
{

  motor1.setSpeed(speed1); // minimum speed 135   max speed 255
  motor2.setSpeed(speed2); // minimum speed 135   max speed 255  
  motor3.setSpeed(speed3); // minimum speed 135   max speed 255
  motor4.setSpeed(speed4); // minimum speed 135   max speed 255 

  Main:

  RangeFront = readRangeFront();
  RangeWall = readRangeWall();

 if (RangeFront < WayTooClose)
 {
  ToCloseWall();
  delay(800);
  Serial.print(" Kliutis Tiesiai");
 }

 if (RangeFront > WayTooClose && RangeWall <= EndOfWall)
 {
  NoObstacles();
  delay(800);
  Serial.print(" Nera Kliuties");
 }


if (RangeFront > WayTooClose && RangeWall > EndOfWall)
{ 
  NoWall();
  delay(800);
  Serial.print(" Baigesi Siena");
}

if (RangeFront <= WayTooClose && RangeWall <= EndOfWall)
{
  ObstacleFront();
  delay(800);
   Serial.print(" Kliutis Tiesiai");
}

if (RangeFront >= WayTooClose && RangeWall >=EndOfWall)
{
  NoObstacles();
  delay(800);
¬† Serial.print(" IeŇ°ko sienos");
}

goto Main;
}


  


int readRangeFront(){
//SRF05 Output
  delay(10);
  unsigned int rangeFront = DistanceSensor1.ping_cm(); // Send ping to first array's sensor
  DistanceSensor1.timer_stop();
   
  return rangeFront;
}

int readRangeWall(){
//SRF05 Output
  delay(10);
  unsigned int rangeWall = DistanceSensor2.ping_cm(); // Send ping to second array's sensor
  DistanceSensor2.timer_stop();
  return rangeWall;
}

  


void NoObstacles()
{  motor1.run(FORWARD); 
       motor2.run(FORWARD); 
       motor3.run(FORWARD); 
       motor4.run(FORWARD); 
       Serial.println("Nera kliuties");
     
   }

void ObstacleFront()
{ motor1.run(BACKWARD);
       motor2.run(FORWARD); 
       motor3.run(FORWARD);
       motor4.run(BACKWARD);    
       Serial.println("Kliutis tiesiai\n");
     
   
}

void NoWall()
{  motor1.run(FORWARD);
        motor2.run(BACKWARD); 
        motor3.run(BACKWARD);
        motor4.run(FORWARD); 
       Serial.println("Baigesi siena");
    
}

void ToCloseWall()
{   motor1.run(BACKWARD);
        motor2.run(BACKWARD); 
        motor3.run(BACKWARD);
        motor4.run(BACKWARD);
       Serial.println("Per arti siena");
     

 }

I would like robot to stop spinning at the corner if he reaches 90 degrees with wall.

If you mean to stop the robot spinning when the sensor is at 90 degrees to the wall then you should be able to do this, at least approximately.

Think about the geometry of the situation. The sensor will be pointing directly at the wall when the distance between it and the wall is at its smallest. So, if you can establish when that is, perhaps by rotating a few degrees either side of the minimum and measuring the distance to establish the minimum distance then you will have made a start.

Do not expect this method to produce accurate or repeatable results but it is something that you can try.

Hello again. Maybe a dumb question but I cant find an answer.

I put my distance sensors information into an equation for if distance is far its -50 if close 50 for more precise wall speed calculations. Now the problem is so when i put my negative number, for example -50 onto if, its just gets ignored.

For example:

 if (p==-50)
 if (q<=-40)
{
    motor1Speed  = speed_max ;
    motor2Speed = speed_max ;
    motor3Speed  = speed_max ;
    motor4Speed = speed_max ;
}


if (-40 < q <-10)
{
    motor1Speed  = speed_max ;
    motor2Speed = motorNeutral ;
    motor3Speed  = motorNeutral ;
    motor4Speed = motorNeutral ;
}

if (-10 <q <10)
{
    motor1Speed  = motorNeutral ;
    motor2Speed = speed_max ;
    motor3Speed  = motorNeutral ;
    motor4Speed = motorNeutral ;
}

if (10 <q <50)
{
    motor1Speed  = motorNeutral ;
    motor2Speed = motorNeutral ;
    motor3Speed  = speed_max ;
    motor4Speed = motorNeutral ;
}

if (q==50 )
{
    motor1Speed  = motorNeutral ;
    motor2Speed = motorNeutral ;
    motor3Speed  = motorNeutral ;
    motor4Speed = speed_max ;
}

}

I put speed_max to different results, to find out which comes out as truth.

My q=-50

The result should come as all set to speed_max but I get last result when q=50, last one is speed_max. Why is that? How can I fix it? Thank you for any help

if (-40 < q <-10)

if q is greater than -40, the result of the first comparison is true. True is not less than -10, so the complete condition is false.

If q is not greater than -40, the result of the first comparison is false. False is not less than -10, so the complete condition is false.

So, you are wrong about the if being ignored.

You do not determine that a value is in a range that way. You need to determine that the value is above the minimum AND below the maximum.

  if(g > -40 && q < -10)

Sorry I made a mistake there. I tried many numbers to see how it will behave. It was a real code.

here it is:

if (q==-50)
{
    motor1Speed  = speed_max ;
    motor2Speed = speed_max ;
    motor3Speed  = speed_max ;
    motor4Speed = speed_max ;
}


if (-50 < q <-10)
{
    motor1Speed  = speed_max ;
    motor2Speed = motorNeutral ;
    motor3Speed  = motorNeutral ;
    motor4Speed = motorNeutral ;
}

if (-10 <q <10)
{
    motor1Speed  = motorNeutral ;
    motor2Speed = speed_max ;
    motor3Speed  = motorNeutral ;
    motor4Speed = motorNeutral ;
}

if (10 <q <50)
{
    motor1Speed  = motorNeutral ;
    motor2Speed = motorNeutral ;
    motor3Speed  = speed_max ;
    motor4Speed = motorNeutral ;
}

if (q==50 )
{
    motor1Speed  = motorNeutral ;
    motor2Speed = motorNeutral ;
    motor3Speed  = motorNeutral ;
    motor4Speed = speed_max ;
}

same test. my q= -50. getting a result that if (10 <q <50) is the corrent answer.

Sorry I made a mistake there.

And you repeated it. It was wrong the first time. It's wrong the second time, too. Quit making that mistake.

Ok so my q calculations change every 10ms. It determins range to wall. It goes from -50 to 50 depending on how far is it. How should I cover all these numbers in your way? Later on I`ll include second sensor, even more numbers.

my equation goes like this

q=-50 no wall

-50 < q < -10 too far from wall

-10 < q < 10 good distance

10<q<50 too close to wall.

-50 < q < -10 too far from wall

In your math class, that might be true. In your computer programming class, that is NOT true.

Look at the code at the bottom of reply #7, to see how to do the comparison after the bell rings at the end of math class.

Thank you Sir. I wrote like this and finaly it started to work.I really appreciate your effort.

if (p>=-50 )
{
    motor1Speed  = motorNeutral-u ;
    motor2Speed = motorNeutral+u ;
    motor3Speed  = motorNeutral+u ;
    motor4Speed = motorNeutral-u ;
}


if (-50 < p && p <-15)
{
    motor1Speed  = motorNeutral-u ;
    motor2Speed = motorNeutral+u ;
    motor3Speed  = motorNeutral+u ;
    motor4Speed = motorNeutral-u ;
}

if (-15 <p && p <15)
{
    motor1Speed  = speed_max ;
    motor2Speed = speed_max ;
    motor3Speed  = speed_max ;
    motor4Speed = speed_max ;
}

if (15 <p && p <50)
{
    motor1Speed  = motorNeutral-u ;
    motor2Speed = motorNeutral+u ;
    motor3Speed  = motorNeutral+u ;
    motor4Speed = motorNeutral-u ;
}

if (p==50 )
{
    motor1Speed  = motorNeutral-u ;
    motor2Speed = motorNeutral+u ;
    motor3Speed  = motorNeutral+u ;
    motor4Speed = motorNeutral-u ;
}

Hello once again. I came with another question.

I wrote program with one sensor which is directed on right side of car, to the wall.

{
            
 if (p==-50)
{
    motor1Speed  = motorNeutral-u;
    motor2Speed = motorNeutral+u;
    motor3Speed  = motorNeutral+u;
    motor4Speed = motorNeutral-u;
}


if (-50 < p && p <-15)
{
    motor1Speed  = motorNeutral-u ;
    motor2Speed = motorNeutral+u ;
    motor3Speed  = motorNeutral+u ;
    motor4Speed = motorNeutral-u ;
}

if (-15 <p && p <15)
{
    motor1Speed  = speed_max ;
    motor2Speed = speed_max ;
    motor3Speed  = speed_max ;
    motor4Speed = speed_max ;
}

if (15 <p && p <50)
{
    motor1Speed  = motorNeutral-u ;
    motor2Speed = motorNeutral+u ;
    motor3Speed  = motorNeutral+u ;
    motor4Speed = motorNeutral-u ;
}

if (p==50 )
{
    motor1Speed  = motorNeutral-u ;
    motor2Speed = motorNeutral+u ;
    motor3Speed  = motorNeutral+u ;
    motor4Speed = motorNeutral-u ;
}

Everything is great. I add another sensor which pointed towards front, if the obstacle gets close, like in 10cm, motors turn car to right side. I added function aswell to get motors ruining backwards. Everything looks simple, but my robot cant even run like it did before with 1 sensor, starts spinning and stuff.

if (p==-50 && q!=50)
{
    motor1Speed  = motorNeutral-u;
    motor2Speed = motorNeutral+u;
    motor3Speed  = motorNeutral+u;
    motor4Speed = motorNeutral-u;
}

if (p==-50 && q==50)
{
    motor1Speed  = speed_max;
    motor2Speed = speed_max;
    motor3Speed  = speed_max;
    motor4Speed = speed_max;
}


if (-50 < p && p <-15 && q!=50)
{
    motor1Speed  = motorNeutral-u ;
    motor2Speed = motorNeutral+u ;
    motor3Speed  = motorNeutral+u ;
    motor4Speed = motorNeutral-u ;
}

if (-50 < p && p <-15 && q==50)
{
    motor1Speed  = speed_max;
    motor2Speed = speed_max;
    motor3Speed  = speed_max;
    motor4Speed = speed_max;
}

if (-15 <p && p <15 && q!=50)
{
    motor1Speed  = speed_max ;
    motor2Speed = speed_max ;
    motor3Speed  = speed_max ;
    motor4Speed = speed_max ;
}

if (-15 <p && p <15 && q==50)
{
    motor1Speed  = speed_max;
    motor2Speed = speed_max;
    motor3Speed  = speed_max;
    motor4Speed = speed_max;
}

if (15 <p && p <50 & q!=50)
{
    motor1Speed  = motorNeutral-u ;
    motor2Speed = motorNeutral+u ;
    motor3Speed  = motorNeutral+u ;
    motor4Speed = motorNeutral-u ;
}

if (15 <p && p <50 && q==50)
{
     motor1Speed  = speed_max;
    motor2Speed = speed_max;
    motor3Speed  = speed_max;
    motor4Speed = speed_max;
}

if (p==50 && q!=50 )
{
    motor1Speed  = motorNeutral-u ;
    motor2Speed = motorNeutral+u ;
    motor3Speed  = motorNeutral+u ;
    motor4Speed = motorNeutral-u ;
}

if (p==50 && q==50 )
{
    motor1Speed  = speed_max;
    motor2Speed = speed_max;
    motor3Speed  = speed_max;
    motor4Speed = speed_max;
}




}

{
   if(motor1Speed > 255)
      motor1Speed = 255;
    else if(motor1Speed <0)
      motor1Speed = 0;
      
    // limit rightMotorSpeed
    if(motor2Speed > 255)
      motor2Speed = 255;
    else if(motor2Speed < 0)
      motor2Speed = 0;

      // limit leftMotorSpeed
    if(motor3Speed > 255)
      motor3Speed = 255;
    else if(motor3Speed < 0)
      motor3Speed = 0;
      
    // limit rightMotorSpeed
    if(motor4Speed > 255)
      motor4Speed = 255;
    else if(motor4Speed < 0)
      motor4Speed = 0;
}


       motor1.setSpeed(motor1Speed);
       motor2.setSpeed(motor2Speed);
       motor3.setSpeed(motor3Speed);
       motor4.setSpeed(motor4Speed);

       if(q>=-50 && q<50 && p>-50 &&p<=50)
       {

    motor1.run(FORWARD);
    motor2.run(FORWARD);
    motor3.run(FORWARD);
    motor4.run(FORWARD);
       }

 if(q==50 && p!=-50)
 {
    motor1.run(BACKWARD);
    motor2.run(FORWARD);
    motor3.run(FORWARD);
    motor4.run(BACKWARD);
 }

Do I need some kind of delays or something? Maybe my sensors information gets interrupted by another sensor. Please help (p,q ==50 ~20cm ; p,q ==0 ~15cm ; p,q == -50 ~8cm). Btw if It sees obstacle in front, goes to the right correctly.

It may not be obvious to you, but the names p and q don’t mean squat.

http://snippets-r-us.com is the place to take your snippets. You are not paying for bandwidth here; squander it.

Not sure if understood correctly but here is my whole program code:

Main program:

#include <AFMotor.h>
#include <NewPing.h>
#include "const.h"
#include "follow_wall.h"
#include "check_front.h"
#include "drive.h"

void setup()
{
    Serial.begin(9600);
  
    // ultrasonic sensor pin configurations
    pinMode(ultrasonic2TrigPin1, OUTPUT);
    pinMode(utlrasonic2EchoPin1, INPUT);
    pinMode(ultrasonic2TrigPin2, OUTPUT);
    pinMode(utlrasonic2EchoPin2, INPUT);
  
     
}


void loop()
{

  Main:
  
    debugOutput(); // prints debugging messages to the serial console
    
    if(millis() - timeLoopDelay >= loopPeriod)
    {
        readUltrasonicSensors2(); // read and store the measured distances

        readUltrasonicSensors1(); // read and store the measured distances
        
         followWall();   

        checkFront();
    
        drive();

       
        timeLoopDelay = millis();

goto Main;
    }
}


void readUltrasonicSensors1()
{
    // ultrasonic 1
    digitalWrite(ultrasonic2TrigPin1, HIGH);
    delayMicroseconds(10);                  // must keep the trig pin high for at least 10us
    digitalWrite(ultrasonic2TrigPin1, LOW);
    
    ultrasonic2Duration1 = pulseIn(utlrasonic2EchoPin1, HIGH);
    ultrasonic2Distance1 = (ultrasonic2Duration1/2)/3;

}

void readUltrasonicSensors2()
{
    // ultrasonic 2
    digitalWrite(ultrasonic2TrigPin2, HIGH);
    delayMicroseconds(20);                  // must keep the trig pin high for at least 10us
    digitalWrite(ultrasonic2TrigPin2, LOW);
    
    ultrasonic2Duration2 = pulseIn(utlrasonic2EchoPin2, HIGH);
    ultrasonic2Distance2 = (ultrasonic2Duration2/2)/3;




}







void debugOutput()
{
    if((millis() - timeSerialDelay) > serialPeriod)
    {
//        Serial.print("ultrasonic2Distance: ");
//        Serial.print(ultrasonic2Distance);
//        Serial.print("mm");
//        Serial.println();
        
        Serial.print("motor1Speed: ");
        Serial.print(motor1Speed);
        Serial.println();
        
        Serial.print("motor2Speed: ");
        Serial.print(motor2Speed);
        Serial.println();

         Serial.print("motor3Speed: ");
        Serial.print(motor3Speed);
        Serial.println();
        
        Serial.print("motor4Speed: ");
        Serial.print(motor4Speed);
        Serial.println();
        
//        Serial.print("e: ");
//        Serial.print(e);
//        Serial.println();
//
        Serial.print("p: ");
        Serial.print(p);
        Serial.println();
        


             Serial.print("u: ");
        Serial.print(u);
        Serial.println();

        Serial.print("q: ");
        Serial.print(q);
        Serial.println();
        
   
             Serial.print("o: ");
        Serial.print(o);
        Serial.println();
        

        
        Serial.println();
        
        timeSerialDelay = millis();

    }

    
}

Front sensor:

void checkFront()
{




  
    int oldF = f;
    
    f = targetDistance - ultrasonic2Distance1;
    
    q = K_p*f; // p gets bigger when the wall is too close and smaller when it is too far away
    
    
    // limit p
  if(q > 50)
     q = 50;
    else if(q < -50)
      q = -50;
    
   
      
    o = 2 * q ;
    
   
}

Wall sensor:

void followWall()
{




  
    int oldE = e;
    
    e = targetDistance - ultrasonic2Distance2;
    
    p = K_p*e; // p gets bigger when the wall is too close and smaller when it is too far away
   
    
    // limit p
   if(p > 50)
    p = 50;
   else if(p < -50)
    p = -50;
   
      
    u = 2* p ;
    
    
   
}

Variables

//Sukuriame motor shields

AF_DCMotor motor1(1, MOTOR12_1KHZ); // create motor #1, 1KHz pwm
AF_DCMotor motor2(2, MOTOR12_1KHZ); // create motor #2, 1KHz pwm
AF_DCMotor motor3(3, MOTOR12_1KHZ); // create motor #1, 1KHz pwm
AF_DCMotor motor4(4, MOTOR12_1KHZ); // create motor #2, 1KHz pwm





int speed_max = 200; //the maximum "speed" the motors will turn ‚Äď you can‚Äôt put in higher

int motor1Speed;
int motor2Speed;
int motor3Speed;
int motor4Speed;


//Sensoriai

#define MAX_DISTANCE 800 // Maximum distance to ping. Edit the distance as needed.

const int ultrasonic2TrigPin1 = A0;
const int utlrasonic2EchoPin1 = A1;
const int ultrasonic2TrigPin2 = A2;
const int utlrasonic2EchoPin2 = A3;

NewPing DistanceSensor1(ultrasonic2TrigPin1, utlrasonic2EchoPin1, MAX_DISTANCE);
NewPing DistanceSensor2(ultrasonic2TrigPin2, utlrasonic2EchoPin2, MAX_DISTANCE);


//test

const int serialPeriod = 250;       // only print to the serial console every 1/4 second
unsigned long timeSerialDelay = 0;

const int loopPeriod = 100;         // a period of 100ms = a frequency of 10Hz
unsigned long timeLoopDelay   = 0;

// specify the trig & echo pins used for the ultrasonic sensors


int ultrasonic2Distance1;
int ultrasonic2Duration1;
int ultrasonic2Distance2;
int ultrasonic2Duration2;

// PID variables
const int targetDistance = 152; // target distance from wall is 6" (~152mm)
int e; // the error (target distance - measured distance)
int u; // the PID output
int p = 0;
int x=0;
int f; // the error (target distance - measured distance)
int o; // the PID output
int q = 0;



// PID gains
const float K_p = 1;


const int motorNeutral = 155;
const int baseMotorSpeed = 180; // 90 is full speed (once added or subtracted from motorNeutral)

Drive program:

void drive()

{

{
            
 if (p==-50 && q!=50)
{
    motor1Speed  = motorNeutral-u;
    motor2Speed = motorNeutral+u;
    motor3Speed  = motorNeutral+u;
    motor4Speed = motorNeutral-u;
}

if (p==-50 && q==50)
{
    motor1Speed  = speed_max;
    motor2Speed = speed_max;
    motor3Speed  = speed_max;
    motor4Speed = speed_max;
}


if (-50 < p && p <-15 && q!=50)
{
    motor1Speed  = motorNeutral-u ;
    motor2Speed = motorNeutral+u ;
    motor3Speed  = motorNeutral+u ;
    motor4Speed = motorNeutral-u ;
}

if (-50 < p && p <-15 && q==50)
{
    motor1Speed  = speed_max;
    motor2Speed = speed_max;
    motor3Speed  = speed_max;
    motor4Speed = speed_max;
}

if (-15 <p && p <15 && q!=50)
{
    motor1Speed  = speed_max ;
    motor2Speed = speed_max ;
    motor3Speed  = speed_max ;
    motor4Speed = speed_max ;
}

if (-15 <p && p <15 && q==50)
{
    motor1Speed  = speed_max;
    motor2Speed = speed_max;
    motor3Speed  = speed_max;
    motor4Speed = speed_max;
}

if (15 <p && p <50 & q!=50)
{
    motor1Speed  = motorNeutral-u ;
    motor2Speed = motorNeutral+u ;
    motor3Speed  = motorNeutral+u ;
    motor4Speed = motorNeutral-u ;
}

if (15 <p && p <50 && q==50)
{
     motor1Speed  = speed_max;
    motor2Speed = speed_max;
    motor3Speed  = speed_max;
    motor4Speed = speed_max;
}

if (p==50 && q!=50 )
{
    motor1Speed  = motorNeutral-u ;
    motor2Speed = motorNeutral+u ;
    motor3Speed  = motorNeutral+u ;
    motor4Speed = motorNeutral-u ;
}

if (p==50 && q==50 )
{
    motor1Speed  = speed_max;
    motor2Speed = speed_max;
    motor3Speed  = speed_max;
    motor4Speed = speed_max;
}




}

{
   if(motor1Speed > 255)
      motor1Speed = 255;
    else if(motor1Speed <0)
      motor1Speed = 0;
      
    // limit rightMotorSpeed
    if(motor2Speed > 255)
      motor2Speed = 255;
    else if(motor2Speed < 0)
      motor2Speed = 0;

      // limit leftMotorSpeed
    if(motor3Speed > 255)
      motor3Speed = 255;
    else if(motor3Speed < 0)
      motor3Speed = 0;
      
    // limit rightMotorSpeed
    if(motor4Speed > 255)
      motor4Speed = 255;
    else if(motor4Speed < 0)
      motor4Speed = 0;
}


       motor1.setSpeed(motor1Speed);
       motor2.setSpeed(motor2Speed);
       motor3.setSpeed(motor3Speed);
       motor4.setSpeed(motor4Speed);

       if(q>=-50 && q<50 && p>-50 &&p<=50)
       {

    motor1.run(FORWARD);
    motor2.run(FORWARD);
    motor3.run(FORWARD);
    motor4.run(FORWARD);
       }

 if(q==50 && p!=-50)
 {
    motor1.run(BACKWARD);
    motor2.run(FORWARD);
    motor3.run(FORWARD);
    motor4.run(BACKWARD);
 } 
    
       
 
}

At the main code, you can notice void readUltrasonicSensors2(). I changed time which keeps trig on high. Now it follows wall a little better. But still it is kind of random. Any ideas why?

You don’t understand that loop() is called over and over, do you? Get rid of the f**king goto statement!.

The distance and duration times should NOT be global variables!

The names p and q are still useless! As are all the other one letter names. The one letter names suggest that you do not understand what the purpose of the variable is.

All the f**king blank lines in your code are annoying.

So are the useless curly braces and piss-poor indenting. There is NO excuse for the poor indenting, since the IDE provides a tool, Auto Format, that will fix it for you.

    debugOutput(); // prints debugging messages to the serial console

BEFORE you’ve learned anything? Useless!

I really can not understand why you have ANY ‚Äúdon‚Äôt do anything for a while‚ÄĚ construct. You don‚Äôt walk down the street only opening your eyes now and then, do you? Why is your robot doing that? It should be checking for where the wall is AS OFTEN AS POSSIBLE!

    // limit p
   if(p > 50)
    p = 50;
   else if(p < -50)
    p = -50;

Why? A big error should not happen, because a big error should result in a big correction, and very quickly your robot should be following the wall.

followWall() does NOT make ANYTHING follow a wall.

Your program comments and some of the variable names suggest that you are using PID. But, you aren’t. Why not? And, if you aren’t going to, then why are the useless variables still there?

Hello,

do you think HC-SR04 sensor is good for this kind of project, or should I think about IR-sensors? Robot is acting weird at different kind of walls (wooden and concrete).

hollowltu: Hello,

do you think HC-SR04 sensor is good for this kind of project, or should I think about IR-sensors? Robot is acting weird at different kind of walls (wooden and concrete).

You should definitely think about IR sensors, and just exactly what they sense, and where what they sense is going to come from.

Robots do NOT behave weirdly. Programmers do. If you right the code properly, the robot will behave properly.

That the data that it has to deal with may be less than perfect does not affect the behavior, if the code is written properly.

It is perfectly reasonable to get different readings from a ping sensor depending on what causes the reflection that the sensor gets back.

Hello, again

I read that IR sensors behave even more weird, because the IR sensor information that comes back to sensor can be changed by the same material of walls and even color. Because the light, sent to obstacle reflects from it, and it depends on color, for example white reflects with least difference, from that light that was sent.

And I am thinking about using servo motors. As I read, they are more precise, easy to control and makes no annoying noises. What do you think about that?

I am doing my bachelor depending on all of that, just starting, but still need lots of help and information. So thank everyone of you who puts any effort to it.