PID Controller

Ok, I'm very new to Arduino so I apologise for my ignorance.

I'm currently writing a code for a PID controller. I currently have the PID controller running in a loop so that it keeps iterating until it reaches my desired set point. This all work fine. Now, where I'm stuck is, once I have reached my desired set point (i.e. when the error is zero) I want to keep the controller running for 10 seconds then break from the loop and move to a new set point.

I have been trailing with IF statements but I'm struggling to incorporate the 10 second delay where the code remains running. So far I have only been able to break the loop as soon as I have reached my set point, without the delay.

Basic steps of my code:

  • PID loop runs.
  • Desired set point reached (Error = 0).
  • Code remains running for 10 seconds (or x amount of time).
  • New PID code runs and moves to new desired set point.
  • New set point reached.
  • Program ends.

Any help will be appreciated.

#include "I2Cdev.h"
#include "MPU6050.h"
#include <Servo.h>
#define KP 40            
#define KI 0.03              
#define KD 10              
#define GyroScale 0.00763358778
int16_t GyroX,GyroY,GyroZ = 0;                                         
long Sum_X;
long Average_GyroX;
float AngularVelocity_X;
float Sum_DegX;
unsigned long DT, Start_Time, End_Time;
int Angle, Motor_Start;
float Error, LastError, Integral=0, Derivative;
float Set_Point, Sensor_Value;
float Motor_Control, PID;
MPU6050 Sensor;
Servo MyProject;


void setup() {
Serial.begin(38400);
Sensor.initialize();
Sensor.setFullScaleGyroRange(2);
Serial.println("Testing Device Connection...");
Serial.println(Sensor.testConnection() ? "Sensor Connection Successful" : "Sensor Connection Failed");
for(int i = 0; i < 1000; i++){
Sensor.getRotation(&GyroX, &GyroY, &GyroZ);                                   
Sum_X = Sum_X + GyroX;  
}
Average_GyroX = Sum_X/1000;
delay(2000);
MyProject.attach(9);
Motor_Start = 90;
MyProject.write(Motor_Start);
delay(5000);
}


void loop() {
Start_Time = millis();
Set_Point = 8;                              
AngularVelocity_X = float((GyroX-Average_GyroX)*GyroScale);
Sum_DegX = Sum_DegX + AngularVelocity_X*DT/1000.0;
Sensor_Value = Sum_DegX;
Error = Set_Point - Sensor_Value;

if (Error > 0.3){

Sensor.getRotation(&GyroX, &GyroY, &GyroZ);
Serial.print("Sensor Value: "); 
Serial.println(Sum_DegX);
Serial.print("Error: "); 
Serial.println(Error);

Integral += Error * DT;
Derivative = (Error - LastError)/DT;
PID = KP*Error + KI*Integral + KD*Derivative;
Motor_Control = map(PID, 200, -200, 0, 180);   
MyProject.write(Motor_Control);
Serial.print("PID Error: ");
Serial.println(PID);
LastError = Error;
End_Time = millis();
DT = End_Time - Start_Time;
}
else {
}
}

Any help will be appreciated.

Posting your code, would help people help you.

My apologies. I've uploaded my code.

callumremington:
My apologies. I've uploaded my code.

To make it easy for people to help you please modify your post and use the code button </>
codeButton.png

so your code 
looks like this

and is easy to copy to a text editor. See How to use the Forum

Also please use the AutoFormat tool to indent your code consistently for easier reading.

I have not studied your code but I suspect you need to put the PID calcs into a function that is called from loop(). That will make it much easier to decide whether or not to call the function. It will also allow you to separate the timing code from the PID code.

...R

OP's code :

#include "I2Cdev.h"
#include "MPU6050.h"
#include <Servo.h>
#define KP 40
#define KI 0.03
#define KD 10
#define GyroScale 0.00763358778
int16_t GyroX, GyroY, GyroZ = 0;
long Sum_X;
long Average_GyroX;
float AngularVelocity_X;
float Sum_DegX;
unsigned long DT, Start_Time, End_Time;
int Angle, Motor_Start;
float Error, LastError, Integral = 0, Derivative;
float Set_Point, Sensor_Value;
float Motor_Control, PID;
MPU6050 Sensor;
Servo MyProject;


void setup() {
  Serial.begin(38400);
  Sensor.initialize();
  Sensor.setFullScaleGyroRange(2);
  Serial.println("Testing Device Connection...");
  Serial.println(Sensor.testConnection() ? "Sensor Connection Successful" : "Sensor Connection Failed");
  for (int i = 0; i < 1000; i++) {
    Sensor.getRotation(&GyroX, &GyroY, &GyroZ);
    Sum_X = Sum_X + GyroX;
  }
  Average_GyroX = Sum_X / 1000;
  delay(2000);
  MyProject.attach(9);
  Motor_Start = 90;
  MyProject.write(Motor_Start);
  delay(5000);
}


void loop() {
  Start_Time = millis();
  Set_Point = 8;
  AngularVelocity_X = float((GyroX - Average_GyroX) * GyroScale);
  Sum_DegX = Sum_DegX + AngularVelocity_X * DT / 1000.0;
  Sensor_Value = Sum_DegX;
  Error = Set_Point - Sensor_Value;

  if (Error > 0.3) {

    Sensor.getRotation(&GyroX, &GyroY, &GyroZ);
    Serial.print("Sensor Value: ");
    Serial.println(Sum_DegX);
    Serial.print("Error: ");
    Serial.println(Error);

    Integral += Error * DT;
    Derivative = (Error - LastError) / DT;
    PID = KP * Error + KI * Integral + KD * Derivative;
    Motor_Control = map(PID, 200, -200, 0, 180);
    MyProject.write(Motor_Control);
    Serial.print("PID Error: ");
    Serial.println(PID);
    LastError = Error;
    End_Time = millis();
    DT = End_Time - Start_Time;
  }
  else {
  }
}

You could have the iteration steps incorporated into a function which will then return true or false depending on the convergence.
If then the result is true, you can start another timer for the 10s delay.

something like this :

bool iterateSteps() {

 if (Error > 0.3){

  ....
  ....
 return false;
 }

else return true;


}

The set point could also be passed as an argument to the above function.

Thanks guys.

So I have included my PID Controller in a function which can be called from the loop. However, I'm struggling to write a code in the loop so my PID Controller works as desired. I'm assuming it will encompass IF statements etc but I just can't get my head around it.

What I'm aiming to do is the following:

  • Pass a value of 5 into PID Controller as the desired Set Point.
  • Hold that position for 10 seconds (but keep the code running).
  • Pass a value of -5 into the PID Controller as the desired Set Point.
  • Hold for 5 seconds.
  • Break loop.

I've included my updated code.

Thanks for your help!

// Include Libraries
#include "I2Cdev.h"
#include "MPU6050.h"
#include <Servo.h>

// PID Parameters
#define KP 40            
#define KI 0.03              
#define KD 0              

// Convert to Degrees
#define GyroScale 0.00763358778

// Initialize Variables
int16_t GyroX,GyroY,GyroZ = 0;                                         
long Sum_X;
long Average_GyroX;
float AngularVelocity_X;
float Sum_DegX;
unsigned long DT, Start_Time, End_Time;
int Angle, Motor_Start;
float Error, LastError, Integral=0, Derivative;
float Set_Point, Sensor_Value;
float Motor_Control, PID;
int a;

// Define Sensor
MPU6050 Sensor;

// Servo Object
Servo MyProject;


void setup() {
Serial.begin(38400);

// Initialize Sensor and Set Sensitivity
 Sensor.initialize();
Sensor.setFullScaleGyroRange(2);

// Test Connection
Serial.println("Testing Device Connection...");
Serial.println(Sensor.testConnection() ? "Sensor Connection Successful" : "Sensor Connection Failed");

//Calibrate Sensor
for(int i = 0; i < 1000; i++){
Sensor.getRotation(&GyroX, &GyroY, &GyroZ);                                   
Sum_X = Sum_X + GyroX;  
}
Average_GyroX = Sum_X/1000;

delay(2000);

// Setup Servo Pin
MyProject.attach(9);

//Set Fin to Starting Position
Motor_Start = 90;
MyProject.write(Motor_Start);

// 5 Second Delay to Allow Sensor to Calibrate
delay(5000);
}


void loop(){
if (Error > 0.3){
  a = 5;
}

STUCK HERE!


PID_Control(a);
Serial.print("Sensor Value: "); 
Serial.println(Sum_DegX);
Serial.print("Error: "); 
Serial.println(Error);
}



// PID Function
int PID_Control(int Set_Point) {
Start_Time = millis();
Sensor.getRotation(&GyroX, &GyroY, &GyroZ);                                   
AngularVelocity_X = float((GyroX-Average_GyroX)*GyroScale);
Sum_DegX = Sum_DegX + AngularVelocity_X*DT/1000.0;             
Sensor_Value = Sum_DegX;
Error = Set_Point - Sensor_Value;
Integral += Error * DT;
Derivative = (Error - LastError)/DT;
PID = KP*Error + KI*Integral + KD*Derivative;
Motor_Control = map(PID, 200, -200, 0, 180);   
MyProject.write(Motor_Control);
LastError = Error;
End_Time = millis();
DT = End_Time - Start_Time;
}

callumremington:
So I have included my PID Controller in a function which can be called from the loop.

Have you got your revised program to perform the same as your earlier working program? That should be your starting point.

If this

  • Pass a value of 5 into PID Controller as the desired Set Point.
  • Hold that position for 10 seconds (but keep the code running).
  • Pass a value of -5 into the PID Controller as the desired Set Point.
  • Hold for 5 seconds.

means that you just want to call the PID function with different values after different intervals you could do something like this

if (millis() - PIDchangeTime >= operatingInterval) {
    PIDchangeTime = millis();
    setPoint = newValue;
    operatingInterval = nextOperatingInterval
}
PID_Control(setPoint);

This will keep calling PID_Control with the same setPoint value until the time expires and the value is changed. Then a new time interval will start.

Separately ...

Get into the habit of using the AutoFormat tool to indent your code and make it easier to read. It can make it much easier to find errors.

Don't use single-character variable names as they are almost impossible to find with a Search tool

To my mind you have a lot more than PID code in your PID_Control function. If this was my project I would have at least three functions - readSensor(), PID_Control() and controlMotor(). Keeping things separate like that allows each part to be tested separately.

...R

Robin2:
If this means that you just want to call the PID function with different values after different intervals you could do something like this

Kind of. I've been trying to get my code to run based off the error value in the PID...

  • Program starts.
  • Value of 5 passed to PID Controller (Set Point).
  • Once Set Point has been reached (i.e. Error = 0 OR -0.5 < Error < 0.5), start a timer for 10 seconds but, keep code running.
  • After 10 seconds has elapsed, pass a value of -5 to the PID Controller (New Set Point).
  • Once New Set Point has been reached (i.e. Error = 0 OR -0.5 < Error < 0.5), start a timer for 5 seconds but, keep code running.
  • After 5 seconds has elapsed, end program.

Thanks for your help!

Read up about state machines. You can use events happening to switch states, and one kind of event is detecting a certain amount of time elapsing.

In you application the states seem to be

PID_SETTLING
PID_SETTLED
NEW_SETPOINT

You'd transition from PID_SETTLING to PID_SETTLED on detecting the PID error reaching zero. At this point
record the value of millis() in a variable.

The transistion from PID_SETTLED to NEW_SETPOINT occurs when the difference between millis() and the variable reaches 10000 milliseconds.

In NEW_SETPOINT state determine a new set point and transition immediately to PID_SETTLING.

A common way to code this would be like:

#define PID_SETTLING 0
#define PID_SETTLED 1
#define NEW_SETPOINT 2

byte state = NEW_SETPOINT ;

void loop()
{
  switch (state)
  {
  case PID_SETTLING:  ..... ; break ;
  case PID_SETTLED: .... ; break ;
  case NEW_SETPOINT: ... ; break ;
  }
}

Have you considered looking at the standard Arduino PID Library?

The author has written up a very detailed explanation of how it works: Improving the Beginner’s PID – Introduction « Project Blog

If nothing else, it could give you some ideas.

gfvalvo:
Have you considered looking at the standard Arduino PID Library?

IMHO it just obfuscates matters.

The OP has a relatively simple and transparent piece of code - I reckon he should stick with it.

...R

I have not gone through your code, but maybe, the first step should be to clean things up. Put the Controller in an object, then think about the logic in the loop.

Pseudo code:

PIDController pid;
bool hold_period = false;
uint32_t setpoint_time = 0;  // Time on which setpoint was reached
const uint32_t hold_time = 10000;

loop() {
// Let PID do his thing
pid.handle(); // This is always called to the PID can react to changes during the hold period

// Control when to start and stop the hold period and set the new setpoint
if(!hold_period && pid.getError()==0) {
  hold_period = true;
  setpoint_time = millis();
}

if(hold_period && millis() - setpoint_time > hold_time) {
  hold_period = true;
  pid.setSetPoint(newval);
}
}

Of course, with the terrible handling of multiple files and subfolders, the ArduinoIDE actively discourages clean code ...

Thanks everyone! Everything is up and running.