Arduino Forum

Topics => Robotics => Topic started by: sandert93 on Sep 18, 2019, 01:34 pm

Title: PID adaptive cruise control
Post by: sandert93 on Sep 18, 2019, 01:34 pm
Hello Friends,

i need some help on my project. I am making a robot that can follow another vehicle (adaptive cruise control). i am using a PID-control to keep the distance (setpoint) at 25cm. Everything works except the PID control. when the followed object is less than 25cm my robot speeds up and crashed against the followed robot. when my robot is at more than 25cm, it supposed to go forward but does nothing. Anyways someting wrong with the PID control.

i've tried to switch: double error = setpoint - sensed_output;
with:                          double error = sensed_output - setpoint;

this seems to work, however when i was reading about the working of the PID this should'nt work. can you guys please look at the code en help me? i am probably overseeing someting.

i have only set the P-value at this moment because i was testing the setup.

Code: [Select]
#include <Arduino.h>
#include <Wire.h>

#include <LiquidCrystal_I2C.h>




// PID control setup//
double sensed_output, control_signal;
double setpoint = 25;
double kp = 5;
double ki = 0;
double kd = 0;
int T = 40;
unsigned long last_time;
double total_error, last_error;

// Ultrasonic sensor//
int Trig = 13;
int Echo = 12;
unsigned long duration;
float distance;

// motor setup//
int enableA = 11;
int in1 = 10;
int in2 = 9;

LiquidCrystal_I2C lcd(0x27, 16, 2); 

void setup() {
  pinMode(enableA, OUTPUT);
  pinMode(in1, OUTPUT);
  pinMode(in2, OUTPUT);
  pinMode(Trig, OUTPUT);
  pinMode(Echo, INPUT);
 
  Serial.begin(9600);
  lcd.init();                                                     // initialiseren van de LCD en achtergrondverlichting aan
  lcd.backlight();
 
}

void loop() { 
    sensed_output = sensorFunction();
    //Serial.println(sensed_output);
    lcd.setCursor(0,0);
    lcd.print(sensed_output);
    PID_control();
       
    PWMcontrol(control_signal);
       
 
}


void PID_control(){
    unsigned long current_time = millis();
    int delta_time = current_time - last_time;

        if (delta_time >= T){
          double error = setpoint - sensed_output;
          total_error += error;
            if (total_error >= 255)
                total_error = 255;
            else if(total_error <= 0)
                total_error = 0;
   
    double delta_error = error - last_error;
    control_signal = kp*error + ki*T*total_error + (kd/T)*delta_error;
   
          if(control_signal >= 255)
            control_signal = 255;
          else if(control_signal <= 0)
           control_signal = 0;   
           
    Serial.println(control_signal);     
    last_error = error;
    last_time = current_time;       
    } 
}

int sensorFunction(){
    digitalWrite(Trig, 0);
    delay(2);
    digitalWrite(Trig, 1);
    delayMicroseconds(5);
    digitalWrite(Trig, 0);

    duration = pulseIn(Echo, HIGH);
    float time = duration/2;
    float speed = 0.03470;
    distance = time * speed;

return distance;
}

void PWMcontrol(int control_signal){
    abs(control_signal);
    analogWrite(enableA, control_signal);
    digitalWrite(in1, HIGH);
    digitalWrite(in2, LOW);
    delay(50);
   //Serial.print("vooruit ");
   //Serial.println(control_signal);
   
   // lcd.setCursor(0,0);
   // lcd.print("BackWard speed:");
   // lcd.setCursor(0,1);
   // lcd.print(LCDspeed);
   // lcd.print(" %");
  }

Title: Re: PID adaptive cruise control
Post by: wildbill on Sep 18, 2019, 02:11 pm
Did you consider using an existing PID library?
Title: Re: PID adaptive cruise control
Post by: sandert93 on Sep 18, 2019, 02:23 pm
Yes i did, but i want to try to make one without a lib. if this isnt working i probably will get a lib.
Title: Re: PID adaptive cruise control
Post by: wildbill on Sep 18, 2019, 02:40 pm
Personally, I'd start with the existing library and only switch it out for a home grown one once everything else was working.

PID is pretty simple, but there are some non obvious "gotchas". Have a look at Brett Beauregard's tutorial about it: here. (http://brettbeauregard.com/blog/2011/04/improving-the-beginners-pid-introduction/)
Title: Re: PID adaptive cruise control
Post by: jremington on Sep 18, 2019, 04:35 pm
Quote
however when i was reading about the working of the PID this should'nt work
If it works, it is correct.

In PID literature there is no accepted definition for the sign of the error term, nor is there an accepted convention specifying how the PID output should act to reduce the error.

That is why the most popular Arduino PID (https://playground.arduino.cc/Code/PIDLibrary/) library allows you to specify the signs of those terms.

If you are developing your own code, you simply have to make sure that the PID output acts to reduce the error term, and that the values are reasonable. Otherwise the system will oscillate wildly or blow up.
Title: Re: PID adaptive cruise control
Post by: sandert93 on Sep 18, 2019, 10:13 pm
Alright, so i dont have to worry about the sign of the error? And continue testing with sensed_output minus setpoint instead of setpoint minus output?

The strange thing is that in everyones code the setpoint - input is working can you explaon why its not working with my code?
Title: Re: PID adaptive cruise control
Post by: jremington on Sep 18, 2019, 11:47 pm
I code the error term as (input_value - setpoint), which makes perfect sense. A positive error means input_value is too high.

So "everyone" is not the correct term to use.
Title: Re: PID adaptive cruise control
Post by: sandert93 on Oct 08, 2019, 11:55 am
alright, so now when im tuning the system i got some strange faults.
Code: [Select]
#include <Arduino.h>
#include <Wire.h>

#include <LiquidCrystal_I2C.h>




// PID control setup//
double sensed_output, control_signal;
double setpoint = 25;
double kp = 2;
double ki = 0.35;
double kd = 0.01;
int T = 50;
unsigned long last_time;
double last_error;

// Ultrasonic sensor//
int Trig = 13;
int Echo = 12;
unsigned long duration;
float distance;

// motor setup//
int enableA = 11;
int in1 = 10;
int in2 = 9;

LiquidCrystal_I2C lcd(0x27, 16, 2); 

void setup() {
  pinMode(enableA, OUTPUT);
  pinMode(in1, OUTPUT);
  pinMode(in2, OUTPUT);
  pinMode(Trig, OUTPUT);
  pinMode(Echo, INPUT);
 
  Serial.begin(115200);
  lcd.init();                                                     // initialiseren van de LCD en achtergrondverlichting aan
  lcd.backlight();
 
}

void loop() { 
    sensed_output = sensorFunction();
    Serial.print(sensed_output);
    Serial.print(",");
    Serial.print(setpoint);
    Serial.print(",");
    lcd.setCursor(0,0);
    lcd.print(sensed_output);
    PID_control();
       
    PWMcontrol(control_signal);
       
 
}


void PID_control(){
    unsigned long current_time = millis();
    int delta_time = current_time - last_time;

        if (delta_time >= T){
          double error = sensed_output - setpoint;                          // error for kp
         
          double total_error = total_error + error;                         // total_error for ki
 
          double delta_error = error - last_error;                          // detla_error for kd
   
          control_signal = (kp*error) + (ki*T*total_error) + ((kd/T)*delta_error);
         
 
    last_error = error;
    last_time = current_time;       
    } 
}

int sensorFunction(){
    digitalWrite(Trig, 0);
    delay(2);
    digitalWrite(Trig, 1);
    delayMicroseconds(5);
    digitalWrite(Trig, 0);

    duration = pulseIn(Echo, HIGH);
    float time = duration/2;
    float speed = 0.03470;
    distance = time * speed;

return distance;
}

void PWMcontrol(int control_signal){
    digitalWrite(in1, HIGH);
    digitalWrite(in2, LOW);
     
    if (control_signal <255 & control_signal >0){
    analogWrite(enableA, control_signal);  //set motor speed
    Serial.println(control_signal);
    }
      else{
        if (control_signal >255){
        analogWrite(enableA,255);
        Serial.println(control_signal);
        }
          else{
          analogWrite(enableA,0);
          Serial.println(control_signal);
          }
      }
}


if i hold the robot a few cmters from the setpoint (around 10) he does not have enough torque to spin the wheels. however the errorfault is around 10 so the integral term should increase the output power over time, correct? in my case this doesnt work..

https://ibb.co/wJB9Y2F
here you can see a image i made from the plotter. the red line is the output power, blue is the distance and orange is the setpoint. as you can see the red line(output power) doesnt increase over time.
Title: Re: PID adaptive cruise control
Post by: jremington on Oct 08, 2019, 05:49 pm
Lines like the following don't work as you seem to expect.
Code: [Select]
double total_error = total_error + error;
The keyword "double" declares a new variable named total_error, which wipes out or ignores any information that might have been stored in the space previously allocated as total_error.

Either declare total_error globally (outside of the function) or as "static" within the function.

For tuning PID algorithms, there are some guidelines. Google will find them for your.

Do not start with arbitrary values for Ki and Kd, start with zero instead.

Title: Re: PID adaptive cruise control
Post by: sandert93 on Oct 09, 2019, 12:46 pm
alright thank you, this seems to work! however i have still problems with tuning the system. if i increase kp it goes fine, pull the robot back and he goes to near the setpoint and stops, now increasing Ki (as i can see on the chart the value rise over time so that works) however it keeps overshooting the setpoint because the output value decreases to slow, now i should apply the Kd but it doesnt matter what value i give to the Kd it keeps overshooting the setpoint.

making/tuning a PID is a lot harder than i thought but we will get there!!

here is my new code
Code: [Select]
#include <Arduino.h>
#include <Wire.h>

#include <LiquidCrystal_I2C.h>




// PID control setup//
double sensed_output, control_signal;
double setpoint = 25;

double kp = (0 - 8);
double ki = (0 - 1);
double kd = (0 - 0);

int T = 50;
unsigned long last_time;
double last_error, total_error;

// Ultrasonic sensor//
int Trig = 13;
int Echo = 12;
unsigned long duration;
float distance;

// motor setup//
int enableA = 11;
int in1 = 10;
int in2 = 9;

LiquidCrystal_I2C lcd(0x27, 16, 2); 

void setup() {
  pinMode(enableA, OUTPUT);
  pinMode(in1, OUTPUT);
  pinMode(in2, OUTPUT);
  pinMode(Trig, OUTPUT);
  pinMode(Echo, INPUT);
 
  Serial.begin(115200);
  lcd.init();                                                     // initialiseren van de LCD en achtergrondverlichting aan
  lcd.backlight();
 
}

void loop() { 
    sensed_output = sensorFunction();
    Serial.print(sensed_output);
    Serial.print(",");
    Serial.print(setpoint);
    Serial.print(",");
    lcd.setCursor(0,0);
    lcd.print(sensed_output);
    PID_control();
       
    PWMcontrol(control_signal);
}

void PID_control(){
    unsigned long current_time = millis();
    int delta_time = current_time - last_time;

        if (delta_time >= T){
          double error = setpoint - sensed_output;                   // error for kp
             
          total_error = total_error + error;                         // total_error for ki
           if(total_error <= -255) total_error = -255;
            else if(total_error >= 0) total_error = 0;
       
           
          double delta_error = error - last_error;                   // detla_error for kd
       
          control_signal = (kp*error) + (ki*T*total_error) + ((kd/T)*delta_error);
           
 
    last_error = error;
    last_time = current_time;       
    } 
}

int sensorFunction(){
    digitalWrite(Trig, 0);
    delay(2);
    digitalWrite(Trig, 1);
    delayMicroseconds(5);
    digitalWrite(Trig, 0);

    duration = pulseIn(Echo, HIGH);
    float time = duration/2;
    float speed = 0.03470;
    distance = time * speed;

return distance;
}

void PWMcontrol(int control_signal){
    digitalWrite(in1, HIGH);
    digitalWrite(in2, LOW);
     
    if (control_signal <=255 & control_signal >=0){
    analogWrite(enableA, control_signal);  //set motor speed
    Serial.println(control_signal);
    }
      else{
       if (control_signal >=255){
       analogWrite(enableA, 255);
        Serial.println(control_signal);
        }
          else{
        analogWrite(enableA,0);
          Serial.println(control_signal);
          }
      }
}
Title: Re: PID adaptive cruise control
Post by: jremington on Oct 10, 2019, 03:52 am
Quote
making/tuning a PID is a lot harder than i thought
That is why there are published guidelines to follow.
Title: Re: PID adaptive cruise control
Post by: sandert93 on Oct 10, 2019, 07:42 am
I am following guideliness for this project like:

https://www.teachmemicro.com/arduino-pid-control-tutorial/

http://brettbeauregard.com/blog/2011/04/improving-the-beginners-pid-introduction/

And for tuning:

https://www.google.com/amp/s/www.electronicshub.org/pid-controller-working-and-tuning-methods/%3Famp

I've been searching the internet for more than 2 weeks before asking the question here... And like i said if there are faults in my programs which i dont see the tuning methods will never work. Like in the link i gave it says:
- increase P til oscillation
- increase I til it slowy goes to the setpoint, this may reult in overshoot
- increase D to reduce the overshoot

So i follow all the steps, but my overshoot never reduces...


Title: Re: PID adaptive cruise control
Post by: sandert93 on Oct 12, 2019, 10:15 am
Nobody who can help?
Title: Re: PID adaptive cruise control
Post by: jremington on Oct 14, 2019, 04:43 am
Quote
Nobody who can help?
Not unless you are willing to provide the details necessary to understand the problem.

See "How to use this forum" for hints on how to be successful at getting help.

Otherwise,
Quote
if this isnt working i probably will get a lib.
Title: Re: PID adaptive cruise control
Post by: sandert93 on Oct 14, 2019, 08:12 am
I described the problem in the post before, here is it again:

however i have still problems with tuning the system. if i increase kp it goes fine, pull the robot back and he goes to near the setpoint and stops, now increasing Ki (as i can see on the chart the value rise over time so that works) however it keeps overshooting the setpoint because the output value decreases to slow, now i should apply the Kd but it doesnt matter what value i give to the Kd it keeps overshooting the setpoint. How is it possible that the kd isnt decreasing while kd is set??
Title: Re: PID adaptive cruise control
Post by: jremington on Oct 14, 2019, 04:31 pm
Here is my suggestion again: read the "How to use this forum" post for hints on how to post.

But, if PID is programmed according to convention, these values should never be negative:
Code: [Select]

double kp = (0 - 8);
double ki = (0 - 1);
double kd = (0 - 0);

Have you considered using K values other than integers? What are the extra zeros doing?

Study this overview: https://en.wikipedia.org/wiki/PID_controller (https://en.wikipedia.org/wiki/PID_controller)
Title: Re: PID adaptive cruise control
Post by: sandert93 on Oct 14, 2019, 06:24 pm
Thank you for the overview and the forum rules, i will carefully read them after work.

Here is my suggestion again: read the "How to use this forum" post for hints on how to post.

But, if PID is programmed according to convention, these values should never be negative:
Code: [Select]

double kp = (0 - 8);
double ki = (0 - 1);
double kd = (0 - 0);

Have you considered using K values other than integers? What are the extra zeros doing?

Study this overview: https://en.wikipedia.org/wiki/PID_controller (https://en.wikipedia.org/wiki/PID_controller)
The reason why i made the K-terms negative is because i had to reverse the direction control. In my case the robot was speeding up when the measured distance became smaller. Somewhere in brett beauregard`s:" Improving the beginner`s PID tutorial" i saw this piece of code and gave it a try.
Here is the link: Improving the beginner's PID: Direction Control (http://brettbeauregard.com/blog/2011/04/improving-the-beginners-pid-direction/)

i also tried other numbers than integers, these numbers are for testing purpose to see if the algorithm is working correct.
Title: Re: PID adaptive cruise control
Post by: jremington on Oct 14, 2019, 07:16 pm
Quote
The reason why i made the K-terms negative is because i had to reverse the direction control
Reverse the output direction instead. This option is included in the library that you so desperately avoid.
Title: Re: PID adaptive cruise control
Post by: sandert93 on Oct 14, 2019, 08:00 pm
Reverse the output direction instead. This option is included in the library that you so desperately avoid.
Does changing the output direction not only change the direction of the wheels spinning?
The goal is to follow an object, so changing the output direction would move the robot away from the direction and increasing the error (because the wheels spin the other way) instead of keeping up at the object and decreasing.

Correct me if i`m wrong.
Title: Re: PID adaptive cruise control
Post by: jremington on Oct 14, 2019, 10:37 pm
The PID algorithm does not specify which direction robot wheels should be spinning.

That is something you must do in your control program, using a motor controller. Speed and direction are separate inputs.