Wall follower with adafruit motorshield and HC-SR04

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?

What are you planning to replace with servo motors? More precise than what? Easier to control than what? If you really believe that servo motors "make no annoying noises", I think that a visit to an audiologist is in your (near) future.

i haven't tried them yet. All the time I have been testing DC motors.

All the time I have been testing DC motors.

And a servo motor is?

A DC motor aswell :smiley: I mean I`m using simple brushed DC motors

I mean I`m using simple brushed DC motors

What I'm confused about is why you think adding a feedback mechanism and a servo horn to one electric motor (making it a servo) is going to make it quieter than another electric motor without the feedback mechanism and servo horn.

Eh I get it now. There is no point, since you remove main part from servo is potentiometer removed. So only stock dc brushed motors are left for me. Well I guess I`ll make it working with them since there is no other way

Hello. I started using PID to control my wheels DC more precisely, but I dont get exactly what I wanted. From PID, I expect to get stability in controlling speed, but I dont get it. As my robot goes long way straight, after few seconds (5-7sec) it starts waving, gets close to wall, detects it and goes back again, wave starts growing, instead of becoming more stable and stable as It goes.

I`m using this kind of robot base (http://www.miniarduino.com/wp-content/uploads/2015/05/DIY-Motor-Smart-Robot-2WD-Car-Chassis-Kit-Speed-Encoder-Battery-Box-arduino.jpg)

Arduino Uno + AdaFruit motor shield copy and two HC-Sr04 sensors. As it looks from information that I get from debug, It should get more stable as It goes, but my robot acts differently.

So what is the problem? DC wheels difference or something else? I will post code on top, to see what my PID does. (Kp, Kd, Ki is being tested, but I cant find much difference as I change them, waving dont go away.)


#include <AFMotor.h>
#include <NewPing.h>
#include <PID_v1.h>
#include "const.h"
#include "debug.h"
#include "sensor.h"
#include "drive.h"

void setup()
  Setpoint = 150;                 //mm
  myPID.SetSampleTime(100);       //ms
  // ultrasonic sensor pin configurations
  pinMode(ultrasonic2TrigPin1, OUTPUT);
  pinMode(utlrasonic2EchoPin1, INPUT);
  pinMode(ultrasonic2TrigPin2, OUTPUT);
  pinMode(utlrasonic2EchoPin2, INPUT);

void loop()
  readUltrasonicSensors2(); // read and store the measured distances
  readUltrasonicSensors1(); // read and store the measured distances
    if(Input > 140 && Input < 160)
  myPID.SetTunings(consKp, consKi, consKd);
  myPID.SetTunings(aggKp, aggKi, aggKd);

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

void Compute()
  if (!inAuto) return;
  unsigned long now = millis();
  int timeChange = (now - lastTime);
  if (timeChange >= SampleTime)
    /*Compute all the working error variables*/
   double error = Setpoint - Input;
    ITerm += (ki * error);
    if (ITerm > outMax) ITerm = outMax;
    else if (ITerm < outMin) ITerm = outMin;
    double dInput = (Input - lastInput);

    /*Compute PID Output*/
    Output = kp * error + ITerm - kd * dInput;
    /*Remember some variables for next time*/
    lastInput = Input;
    lastTime = now;

void SetTunings(double Kp, double Ki, double Kd)
  if (Kp < 0 || Ki < 0 || Kd < 0) return;

  double SampleTimeInSec = ((double)SampleTime) / 1000;
  kp = Kp;
  ki = Ki * SampleTimeInSec;
  kd = Kd / SampleTimeInSec;

  if (controllerDirection == REVERSE)
    kp = (0 - kp);
    ki = (0 - ki);
    kd = (0 - kd);

void SetSampleTime(int NewSampleTime)
  if (NewSampleTime > 0)
    double ratio  = (double)NewSampleTime / (double)SampleTime;
    ki *= ratio;
    kd /= ratio;
    SampleTime = (unsigned long)NewSampleTime;

void SetOutputLimits(double Min, double Max)
  if (Min > Max) return;
  outMin = Min;
  outMax = Max;

  if (Output > outMax) Output = outMax;
  else if (Output < outMin) Output = outMin;

  if (ITerm > outMax) ITerm = outMax;
  else if (ITerm < outMin) ITerm = outMin;

void SetMode(int Mode)
  bool newAuto = (Mode == AUTOMATIC);
  if (newAuto == !inAuto)
  { /*we just went from manual to auto*/
  inAuto = newAuto;

void Initialize()
  lastInput = Input;
  ITerm = Output;
  if (ITerm > outMax) ITerm = outMax;
  else if (ITerm < outMin) ITerm = outMin;

void SetControllerDirection(int Direction)
  controllerDirection = Direction;


/*working variables*/
unsigned long lastTime;
double Input, Output, Setpoint;
double ITerm, lastInput;
double consKp=0.05, consKi=0, consKd=0;
double aggKp=1, aggKi=0, aggKd=0;
double Kp, Ki, Kd;
double kp, ki, kd;
double outMin, outMax;
double error;
int SampleTime = 100; //0,1 sec
bool inAuto = false;
PID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT);

#define MANUAL 0
#define AUTOMATIC 1

#define DIRECT 0
#define REVERSE 1
int controllerDirection = DIRECT;

//Motor set
AF_DCMotor motor1(1, MOTOR12_8KHZ); // create motor #2, 1KHz pwm
AF_DCMotor motor2(2, MOTOR12_8KHZ); // create motor #2, 1KHz pwm
int Lmotor;
int Rmotor;
const int motorNeutral = 160;
int speedmax = 255;

// specify the trig & echo pins used for the ultrasonic sensors
const int ultrasonic2TrigPin1 = A0;
const int utlrasonic2EchoPin1 = A1;
const int ultrasonic2TrigPin2 = A2;
const int utlrasonic2EchoPin2 = A3;
#define MAX_DISTANCE 500
NewPing DistanceSensor1(ultrasonic2TrigPin1, utlrasonic2EchoPin1, MAX_DISTANCE);
NewPing DistanceSensor2(ultrasonic2TrigPin2, utlrasonic2EchoPin2, MAX_DISTANCE);

int Front;
int ultrasonic2Duration1;
int ultrasonic2Duration2;

Debug output:

void debugOutput()

  Serial.print("Lmotor: ");

  Serial.print("Rmotor: ");

  Serial.print("Front: ");

  Serial.print("Input: ");

  Serial.print("Output: ");


Drive code:

void drive()
  if (Front > 160) //160mm
  Lmotor =  motorNeutral - Output;
  Rmotor =  motorNeutral + Output;
  if (Front < 160)
    Lmotor = 0;
    Rmotor = speedmax;

  // limit rightMotorSpeed
  if (Rmotor > 200)
    Rmotor = 200;
  else if (Rmotor < 0)
    Rmotor = 0;

  // limit rightMotorSpeed
  if (Lmotor > 200)
    Lmotor = 200;
  else if (Lmotor < 0)
    Lmotor = 0;




Sensor information:

void readUltrasonicSensors1()
  // ultrasonic 2
  digitalWrite(ultrasonic2TrigPin1, HIGH);
  delayMicroseconds(10);                  // must keep the trig pin high for at least 20us
  digitalWrite(ultrasonic2TrigPin1, LOW);

  ultrasonic2Duration1 = pulseIn(utlrasonic2EchoPin1, HIGH);
  Front = (ultrasonic2Duration1 / 2 ) / 3;
void readUltrasonicSensors2()
  // ultrasonic 2
  digitalWrite(ultrasonic2TrigPin2, HIGH);
  delayMicroseconds(10);                  // must keep the trig pin high for at least 10us
  digitalWrite(ultrasonic2TrigPin2, LOW);

  ultrasonic2Duration2 = pulseIn(utlrasonic2EchoPin2, HIGH);
  Input = (ultrasonic2Duration2 / 2 ) / 3;

Thank you for any help.

Trying one thing, I`ll try to measure motor spinning speed and control it with that information, will make them more stable

Hello again. I am jumping back and forward and I just cant make my mind. I need your help. I am making a precise moving robot, which can follow walls with as less mistakes as possible, so I could count perimeter. What Im asking is for advice about motors. Which do you think are the best to do this kind of project?

So far I have Arduino UNO(fake) and adafruit motor shield copy from DK-Electronics. I can`t get my motors move straight, because with all of the coming voltage difference.


Should I try to get better controls of them with encoders, or skip it and get different kind of kit.
Thank you for any help!

so I could count perimeter

How do you count perimeter? One perimeter, two perimeters?

Measure, maybe. But ONLY if you KNOW that n milliseconds of travel time is m millimeters. Which I doubt, since you have no way of knowing how much the robot is slipping.

My English is a little bit broken. Yes I need to measure perimeter with like less than 5% mistake. I need to make is as accurate as I can. First, im trying to make a good wall follower. That is what I`m trying to figure out. How to do it. And now I want to find out, which motors fits me best.

And now I want to find out, which motors fits me best.

You are operating under that mistaken impression that changing motors is going to make a difference. It is not.

Even with slight differences in the motors, you can still make the robot go in a straight line by making slight adjustments in the speed of each motor. Usually, this is done using PID.

I know that I need to use PID, only thing I thought other type of motors might make it easier to control, to make them stable. Typical DC motors that Im using needs to charge full to run smooth (takes some time), depends on voltage and battery charge level and so on. Few people advice me, to make both DC motors with speed modules (encoders) to track their speed, and by doing that, make a code to control motors precisely. Others say, if you want easy controlled precise motors, pick servo continuous rotation motors or other, more expensive ones DC motors, with better stability. Im using PID right now with my code, but it just doesn`t look right, something is missing, and I want to know if motors are this reason. So far as I get from this topic, it is not. Maybe my code is wrong, I dont know, but I tested many times with different variables (kp,ki,kd) and errors keep on growing as the robot goes (instead of making it more stable).

Sensors looks pretty stable so far with small distances, but still I`m thinking of trying Grove IR type sensor. Need the best equipment I can get. Thank you for all this information.