FIrst try with PID and PID library!!

Hi! Just wanted to try PID with my little robot car. The robot car is based on an UNO, two dc motors, a L293D and three HC-SR04. It´s also possible to connect a HC-06 to control it by Bluetooth. So, the car has been remotely controlled, a wall follower and a maze solver and so on. I haven´t been using PID in any of these projects but now wanted to try that in a simple way by just reading distance from the right HC-SR04 and see if the car could follow a left wall at a desired distance.

I´ve tried to implement the PID library basic example into parts of my previous code just to achieve the left wall following. The distance is measured in inches. The pwm values pwmSpeedRight = 100 and pwmSpeedLeft = 132 makes the car go straight forward, well almost.

Since I don´t have the car where I am right now I can´t try the code in real. So I just thought posting it here and see if someone have a comment maybe could be a good thing?! It´s not unlikely that I´m far out from understanding how to use this library - I´m just guessing :smiley: ?!

#include <PID_v1.h>

//Define Variables we'll be connecting to
double Setpoint, Input, Output;

//Specify the links and initial tuning parameters
PID myPID(&Input, &Output, &Setpoint,2,5,1, DIRECT);

const int leftForward = 2;
const int leftBackward = 3;
const int rightForward = 4;
const int rightBackward = 5;

const int rightPWM = 10;
const int leftPWM = 6;

int pwmSpeedRight = 100;
int pwmSpeedLeft = 132;

const int trigPin2 = A2;
const int echoPin2 = A1;
long duration2; 
int distance2;


void setup() 
{
  pinMode(leftForward , OUTPUT);
  pinMode(leftBackward , OUTPUT);
  pinMode(rightForward , OUTPUT);
  pinMode(rightBackward , OUTPUT);
  pinMode(trigPin2, OUTPUT);
  pinMode(echoPin2, INPUT);
  pinMode(rightPWM, OUTPUT);
  pinMode(leftPWM, OUTPUT);

  //initialize the variables we're linked to
  Input = distance2;
  Setpoint = 8;

  //turn the PID on
  myPID.SetMode(AUTOMATIC);

}
void loop() 
{
  distance2Left();
  LeftRightForwardHigh();
  Input = distance2;
  myPID.Compute();
  analogWrite(rightPWM,Output);
  analogWrite(leftPWM, pwmSpeedLeft);

//delay(50);
}   //to void loop

void distance2Left()
{
  digitalWrite(trigPin2, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin2, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin2, LOW);  
  duration2 = pulseIn(echoPin2, HIGH);
  distance2 = duration2*0.034/2; 
}
void LeftRightForwardHigh()
{
  digitalWrite(leftForward , HIGH);
  digitalWrite(leftBackward , LOW);
  digitalWrite(rightForward , HIGH);
  digitalWrite(rightBackward , LOW);  
}

I mean "right wall" !! :slight_smile:

And I also mean centimeter - not inch!! :slight_smile:

Look up "PID tuning".

You will need to choose reasonable values for Kp, Ki, and Kd, based on guidelines and experience with the functioning robot.

The values in this line are almost certainly unreasonable:

PID myPID(&Input, &Output, &Setpoint,2,5,1, DIRECT);

Ok! So 2, 5 and 1 are not ok values in this case?

I didn´t really inderstand them - so I´ll have to dig into PID tuning then!

There is some simple PID code in this link.

...R

i looked at the code in the How to limit the revolutions thread ...

shouldn't the PID result be signed to indicate the motor direction to allow the motor voltage to be reversed before reaching the target to force the motor to slow faster?

gciurpita:
i looked at the code in the How to limit the revolutions thread ...

shouldn't the PID result be signed to indicate the motor direction to allow the motor voltage to be reversed before reaching the target to force the motor to slow faster?

Probably with the motor and load being used with that code it was not necessary. Reversing the voltage would be like jamming on the brakes.

Most small motors stop very quickly when you disconnect the power. Perhaps if the motor was driving a heavy flywheel you would need active braking.

Every control system has to be tailored to the physical system it is controlling.

...R

The code now looks like this. As said earlier it is a robot-like car supposed to follow a right hand wall at a distance of 10 cm (Setpoint) with one HC-SR04 distance sensor measuring the distance to the wall. So the Input is the measured distance and the Output a change in pwm only for the right wheel (the left wheel stays at the pre-set pwm value 208). The changing value for the right wheel is the pre-set value 200 +- Output. I have two PID functions, one Direct and one Reverse to be able to measure on both sides(?) of 10 cm.

The system now behaves and reacts as I want (when just measuring distance and looking at serial print values) - but to slow though, in real the car gets stuck at the wall all the time. I´ve been trying many tuning parameters (both with and without Ki and Kd) but I want a more rapid action. Any suggestions - or is it not doable in this kind of system with dc motors and HC-SR04?

#include <PID_v1.h>
double SetpointDirect, InputDirect, OutputDirect;
double SetpointReverse, InputReverse, OutputReverse;
PID DirectPID(&InputDirect, &OutputDirect, &SetpointDirect,7,0.1,0.01, DIRECT);
PID ReversePID(&InputReverse, &OutputReverse, &SetpointReverse,7,0.1,0.01, REVERSE);

const int leftForward = 2;
const int leftBackward = 3;
const int rightForward = 4;
const int rightBackward = 5;

const int rightPWM = 10;
const int leftPWM = 6;

int pwmSpeedRight = 200;
int pwmSpeedLeft = 208;

const int trigPin3 = A2; 
const int echoPin3 = A1;
long duration3; 
int distance3;

void setup() 
{
  Serial.begin(9600);
  pinMode(leftForward , OUTPUT);
  pinMode(leftBackward , OUTPUT);
  pinMode(rightForward , OUTPUT);
  pinMode(rightBackward , OUTPUT);
  pinMode(trigPin3, OUTPUT);
  pinMode(echoPin3, INPUT);
  pinMode(rightPWM, OUTPUT);
  pinMode(leftPWM, OUTPUT);

  //Input = distance3;
  SetpointDirect = 10;
  SetpointReverse = 10;

  DirectPID.SetMode(AUTOMATIC);
  ReversePID.SetMode(AUTOMATIC);
  DirectPID.SetOutputLimits(0,55);
  ReversePID.SetOutputLimits(0,55);
}
void loop() 
{
  /*LeftRightForwardHigh();
  analogWrite(rightPWM,pwmSpeedRight);
  analogWrite(leftPWM, pwmSpeedLeft);*/

  distance3Right();
  LeftRightForwardHigh();

  if (distance3 == 10)
  {
  analogWrite(rightPWM,pwmSpeedRight);
  analogWrite(leftPWM, pwmSpeedLeft);

  Serial.print("   distance3 = ");
  Serial.print(distance3);
  Serial.print(" cm ");
  Serial.println();
  }
  if (distance3 < 10)
  {
  InputDirect = distance3;
  DirectPID.Compute();
  analogWrite(rightPWM,pwmSpeedRight+OutputDirect);
  analogWrite(leftPWM, pwmSpeedLeft);

  Serial.print("   OutputDirect = ");
  Serial.print(OutputDirect);
  Serial.print("   pwmSpeedRight+OutputDirect = ");
  Serial.print(pwmSpeedRight+OutputDirect);
  Serial.print("   distance3 = ");
  Serial.print(distance3);
  Serial.print(" cm ");
  Serial.println();
  }
  if (distance3 > 10)
  {
  InputReverse = distance3;
  ReversePID.Compute();
  analogWrite(rightPWM,pwmSpeedRight-OutputReverse);
  analogWrite(leftPWM, pwmSpeedLeft);

  Serial.print("   OutputReverse = ");
  Serial.print(OutputReverse);
  Serial.print("   pwmSpeedRight-OutputReverse = ");
  Serial.print(pwmSpeedRight-OutputReverse);
  Serial.print("   distance3 = ");
  Serial.print(distance3);
  Serial.print(" cm ");
  Serial.println();
  }
//delay(20);
}

void distance3Right()
{
  digitalWrite(trigPin3, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin3, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin3, LOW);  
  duration3 = pulseIn(echoPin3, HIGH);
  distance3 = duration3*0.034/2; 
}
void LeftRightForwardHigh()
{
  digitalWrite(leftForward , HIGH);
  digitalWrite(leftBackward , LOW);
  digitalWrite(rightForward , HIGH);
  digitalWrite(rightBackward , LOW);  
}

Slow the car down, so that it has time to react to the sensor input.

Increase the serial Baud rate from 9600 to 15200, so you don't spend so much time printing.

Good suggestions! Thanks!!

I can of course turn off serial printing - its just for testing purpose to see reactions on different values!

First I had the preset speed at a lower value - but increased it to not get to the point where the wheel stopped turning (like below 60-70 pwm)! I´ll try like 150?!!

Some thoughts:

You can avoid integral and all the windup issues it causes so set integral to Zero!
How?
When your car is on setpoint or the desired distance you will want to drive straight so your turn value will be zero. Integral shifts the turn value so setpoint can be reached but you know that when the setpoint is reached you don't need to turn right or left anymore.

now Derivative is messy when you have an unstable sensor reading. The derivative will compare current reading to the past reading and "Kick" the output one way or another the changes between readings so if you wish to use derivative with the HC-SR04 you will want to average many readings together and some kind of filter to remove any unwanted spikes that could trigger aggressive "kicking".

Interesting enough your wall following concept is similar to a balancing bot. And you could transfer the same concepts over to your code. My balancing bot has a setpoint of Zero as when I am at zero degrees my bot is balanced. and my power is also at zero. The same principle works for your wall follower. when you're at setpoint your desire to turn is zero.
Take a peek at this video I did long ago for some help. How to Balance Robot PID tutorial in under 2 minutes! I did add integral at the end but the bot could balance without it. You may find the derivative jitter can't be solved even with filtering your input to remove noise and so you may keep your derivative at zero also.

Also, something of note, in order to have a good response with turning you may want to jump to a power level enough to cause motion. In other words, you may find a dead area between turning left or right. you want that dead area to be as minimal as possible. For example, when I control my bot in tank mode to get it to turn requires much more power to the wheels than when I call it to just move forward. and getting it to move forward I need to be at least have a pulse width of 40 (0-255) so I need to skip over to 40 to get any action with the motors I am using. While this may not be needed it could help you tune your bot to be responsive to requests from the Proportional control you will be using.

Z

A little snip of code to easily control a tank-style robot.

/*
 *  For this example I am using RC pulse which ranges from 0 to 1000 having th RC remote 
 *  centered the output would be around 500. i shift this to provide a value from -500 to +500
 *  so X and Y will be looking for a value between +- 500
 *  In turn the output for the Left motor "LeftDrive" and right motor "RightDrive" will scale between +-500
 *  The Thottle value is a max power and should scale between 0 and 500
 */ 

int deadZone =  50;

void XYtoTank(int16_t X, int16_t Y, int16_t* LeftDrive, int16_t* RightDrive,int16_t Throttle){
  int16_t LeftD,RightD,V;
  float DriveScaler;
  //mix throttle and direction
  LeftD = (X - Y) ;
  RightD = (X + Y) ;
  DriveScaler = (max(1, max(abs((float)LeftD / 500.0), abs((float)RightD / 500.0))));
  V = (int16_t)constrain((float)LeftD / DriveScaler, -500, 500);
  if(deadZone)V = TankDeadzone(V, deadZone);
  V = (Throttle == -1) ? V: map(V,0,500,0,Throttle); // Adjust Throttle Controlls Max Speed
  *LeftDrive =  V ;
  V = (int16_t)constrain((float)RightD / DriveScaler, -500, 500);
  if(deadZone)V = TankDeadzone(V, deadZone);
  V = (Throttle == -1) ? V: map(V,0,500,0,Throttle); // Adjust Throttle Controlls Max Speed
  *RightDrive =  V;
  TankLeftDrive = *LeftDrive;
  TankRightDrive = *RightDrive;
}
int16_t TankDeadzone(int16_t V, uint16_t deadzone){
  int16_t absV = abs(V);
  absV = (absV < 3) ? 0 : map(absV, 3, 500, deadzone ,500) ; // Inserts a Deadzone
  V = (V >= 0) ? absV :  -absV; // Restores a FullRange Value +-
  return(V);
}

Many thanks for your thoughts and suggestions on this topic zhomeslice!

I´ll have a look at it - and will be back for a report!!

When I reach, or pass, Setpoint its not sure though that the car is exactly in straight forward position. Thats where I thought maybe integral and derivative could kick in?!

Maybe I should skip the go straight option in the code and just let PID work on both sides of Setpoint?!!

Great video zhomeslice! A perfect way to really see the effects of PID!!

dwnld99:
When I reach, or pass, Setpoint its not sure though that the car is exactly in straight forward position. Thats where I thought maybe integral and derivative could kick in?!

Maybe I should skip the go straight option in the code and just let PID work on both sides of Setpoint?!!

proportional will bring the bot back to straight to follow along the path/wall. having it too hot your bot will oscillate having it too weak it will not catch up.
Integral would only be used if your motors are not balanced integral adjusts the output off of setpoint so let us say 0 is properly balanced straight drive and 10 cm is setpoint with the bot at 10 cm the turn command with proportional only os 0. now lets say that the right motor is more powerful than the left and so it swings to the right. Integral is designed to accommodate this deviation but be warned it has no clue how much integral influence is needed so if you add integral to your equation and have your bot setting away from the desired setpoint the integral influence starts adding to the output so that zero is no longer at setpoint but the integral may wind up and have 50 added to the output when you are on setpoint. in your case, it is important to reset integral windup to zero before and possibly keep it there until you have gained a lock-on setpoint. the only drawback if you are not using integral is that you may have the setpoint at 10cm and because of the unbalanced motors you actually follow the wall at 12cm to cause the proportional to drive the motors in a straight line.
Now for Derivative. Using a small amount of derivative to clamp down on setpoint is possibly a good thing you must remember Derivative is a measurement of change between readings if you are maintaining a setpoint derivative will have no effect on the output. it is only when change occurs is when derivative take a role in the output.
You may find this helpful.
The PID_v1 library has a flaw in that it uses a fixed time window it sets and the math is always based on that fixed time window. My balancing bot fails terribly using the PID_v1 code provided
attached is my PID_v1 modifications

Z

PID_v1.cpp (11.3 KB)

PID_v1.h (3.49 KB)

the way I've used PID is

  • error = target - position
  • that error is used to control the motor, the motor voltage. the voltage will reduce as the error gets smaller
  • but it's likely that there will be overshoot unless the Kp determine reduces the motor voltage early enough
  • the derivative term is the speed: change in position / change in time ((p0-p1)/(t0-t1)
  • the derivative is subtracted from the proportional term to anticipate the speed approaching the target
  • the resulting subtraction of the derivative from the error can result in a negative motor voltage, braking
  • the integral term is unnecessary in digital systems or for position control
  • if PD were used for speed control, there would be insufficient motor voltage as the error gets small
  • the integral term results in a residual output when the error (P - D) is zero to maintain motor speed

Thanks again zhomeslice!!

Thanks gciurpita!!

i'm not sure PID is well suited for position control. I think it's better suited for maintaining a dynamic value, such as speed which can constantly be adjusted. We used PID to control a timing loop at work. But we were told there are better approaches than PID (don't know what they are aside from Wiener filters).

gciurpita:
i'm not sure PID is well suited for position control. I think it's better suited for maintaining a dynamic value, such as speed which can constantly be adjusted. We used PID to control a timing loop at work. But we were told there are better approaches than PID (don't know what they are aside from Wiener filters).

Speed is primarily the integral portion of PID as you don't know exactly how much power is needed to achieve the speed. also, note Integral has troubles with windup which is when the setpoint cant be achieved even with additional power applied and precautions must be taken to reset and limit this windup. When you know that at setpoint you are going to drive straight then integral has no purpose.
Now Proportional and Derivative are ideal for positioning control as a deviation will cause an instant adjustment to the motors requested power turning the bot back towards the setpoint.
@dwnld99 s case he/she would be using PD instead of PID to guide the bot.
Z