Go Down

Topic: PID adaptive cruise control (Read 3564 times) previous topic - next topic

sandert93

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(" %");
  }


wildbill

Did you consider using an existing PID library?

sandert93

Yes i did, but i want to try to make one without a lib. if this isnt working i probably will get a lib.

wildbill

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.

jremington

#4
Sep 18, 2019, 04:35 pm Last Edit: Sep 18, 2019, 04:39 pm by jremington
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 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.

sandert93

#5
Sep 18, 2019, 10:13 pm Last Edit: Sep 18, 2019, 10:15 pm by sandert93
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?

jremington

#6
Sep 18, 2019, 11:47 pm Last Edit: Sep 18, 2019, 11:49 pm by jremington
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.

sandert93

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.

jremington

#8
Oct 08, 2019, 05:49 pm Last Edit: Oct 08, 2019, 05:52 pm by jremington
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.


sandert93

#9
Oct 09, 2019, 12:46 pm Last Edit: Oct 09, 2019, 12:47 pm by sandert93
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);
          }
      }
}

jremington

Quote
making/tuning a PID is a lot harder than i thought
That is why there are published guidelines to follow.

sandert93

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...



sandert93


jremington

#13
Oct 14, 2019, 04:43 am Last Edit: Oct 14, 2019, 04:46 am by jremington
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.

sandert93

#14
Oct 14, 2019, 08:12 am Last Edit: Oct 14, 2019, 08:20 am by sandert93
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??

Go Up