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