Hello everyone
Few weeks ago i posted a topic about synchronization of two actuators. I received many useful answers, so i decided to implement PID controller.
How it works: I have two actuators, one is master, second is slave (follower). One button is for extension (when we press this button and keep him HIGH, the master starts to move forward) another one is for retraction. Basically, with buttons we are controlling only master, and slave is always following the master (so its always a liitle bit behind of master). Actuators are electrical with potentiometers. They are controlled by one arduino MEGA and two H-bridges (one H-bridge per actuator).
So it works pretty fine now, but I can still see some oscillations. The problem is, when master is stopped, and slave have reached his position, sometimes he begins to move forward and backwards constantly. I wonder what may be wrong in my code. Some useful information: The values from both potentiometers are not the same, to be more precisly, value from potentiometers, when master and slave are fully extended, are not the same (they are oscillating between 443-445).
I would be very grateful for feedback and if u could take a look.
Thanks!
Code:
#include <PID_v1.h> //include PID library (załącz bibliotekę regulacji PID (http://playground.arduino.cc/Code/PIDLibrary))
double value, value1; //values, which will be readed from potentiometers of master and slave (wartosc, ktora bedzie odczytana z potencjometru mastera)
const int buttonPos = 42; //button for extension of the master (przycisk do wysuwu mastera z przypisaniem pinu)
const int buttonPos2 = 43; //button for retrack the master (przycisk do wsuwu mastera)
double value_imp, value1_imp; //values, which will be scalled from master and slave (wartosc, ktora zostanie przeskalowana z mastera i slave)
double Setpoint, Input, Output; //arguments of PID regulator (argumenty funkcji regulatora PID)
double consKp=1.00, consKi=0.00, consKd=0.00; //parameteres of PID (nastawy)
double junk, junk1;
int pidZeroLimit = 1, pidOutputMinValue = 150, pidOutputValue = 0; //min. speed for slave, initial speed
PID myPID(&Input, &Output, &Setpoint, consKp, consKi, consKd, DIRECT); //initialization of pid function (zainicjowanie funkcji)
void setup()
{
pinMode (2,OUTPUT); //PWM master
pinMode (3,OUTPUT); //PWM follower
pinMode (22,OUTPUT); //extension of master wysuw master
pinMode (23,OUTPUT); //retract of master wsuw mastera
pinMode (24,OUTPUT); //extension of slave
pinMode (25,OUTPUT); //retract of slave
pinMode (42,INPUT); //button for extension
pinMode (43,INPUT); //button for retract
analogWrite(2,150); //speed of master
junk = analogRead(A0);
Setpoint = analogRead(A0); //Setpoint - value from master pot., that we want to Input to maintain (wartosc z pot. mastera, do której będzie dążył slave)
junk1 = analogRead(A1);
Input = analogRead(A1); //Input - value from slave pot., that we are trying to control (to wartosc z potencjometru slave'a, którą chycemy kontrolować)
myPID.SetTunings(consKp, consKi, consKd); //dictate the dynamic behavior of the PID (ustawienie nastaw regulatora)
myPID.SetOutputLimits(0, 255); //configurate the speed limits (ustawienie limitów prędkości)
Output = 0;
myPID.SetMode(AUTOMATIC);
Serial.begin(9600);
}
void loop()
{
value = analogRead(A0); //read the value from master pot. (sczytaj wartosc z potencjometr mastera)
value_imp = map(value, 944, 29, 255, 0); //scale these value to receive lesser resolution (zeskaluj wartosci (994 - max wysuw, 29 - max wsuw) na 255-0)
Setpoint = value_imp; //make a Setpoiunt from scalled value of pot. master (ustaw setpoint, czyli wartosc, jaka input bedzie musial osiagnac)
value1 = analogRead(A1); //make the same cycle for slave
value1_imp = map(value1, 944, 29, 255,0);
Input = value1_imp;
myPID.Compute(); //
//sterowanie masterem (master controlling)
if(digitalRead(buttonPos) == HIGH) //extension if button is pressed (wysuw, jeśli przycisniemy przycisk)
{
digitalWrite(22,HIGH); //exntension is HIGFH
digitalWrite(23,LOW); //retract is LOW
//digitalWrite(24,HIGH);
//digitalWrite(25,LOW);
}
else if(digitalRead(buttonPos2) == HIGH) //retract if button is pressed
{
digitalWrite(22,LOW);
digitalWrite(23,HIGH);
//digitalWrite(24,LOW);
//digitalWrite(25,HIGH);
}
else //if nothing is pressed do nothing (jesli nic nie przycisniete to nic nie rob)
{
digitalWrite(22,LOW);
digitalWrite(23,LOW);
}
//slave
int deltaAnalogValue = Setpoint - Input; //Measure the difference between setpoint and input (Mierz roznice pomiedzy setpoint a input)
if (Output > pidZeroLimit) //if output is greater than limit than (jesli output jest wiekszy od obszaru tolerancyjneg)
{
pidOutputValue = pidOutputMinValue + abs(Output) ; //add the output distance to minimum speed (dodaj do predkosci docelowej predkosc minimalna oraz output)
//the greater is output, the actuator should exten/retract faster (im wekszy output tym szybciej silownik powinien sie wysuwac/wsuwac)
pidOutputValue = constrain(pidOutputValue, 0, 255); //the value shouldnt be greater than 255 or lesser than 0 (wartosc ta nie moze przekroczyc 0-255)
if (deltaAnalogValue > 0) //if the difference is >0: (jesli wartosc roznicy przekracza 0, to:)
{
digitalWrite(24,HIGH); //extend the master
digitalWrite(25,LOW); //
analogWrite(3, pidOutputValue); //speed of slave (Przypisz do pwmu slave'a predkosc)
}
}
else if(Output < pidZeroLimit) //same as above but for slave
{
pidOutputValue = pidOutputMinValue + abs(Output);
pidOutputValue = constrain(pidOutputValue, 0, 255);
if (deltaAnalogValue < 0)
{
digitalWrite(24,LOW);
digitalWrite(25,HIGH);
analogWrite(3, pidOutputValue); //pwm slave
}
}
else
{
digitalWrite(24,LOW);
digitalWrite(25,LOW);
pidOutputValue = 0;
}
Serial.println("Output");
Serial.println(Output);
Serial.println("Input");
Serial.println(Input);
Serial.println("Setpoint");
Serial.println(Setpoint);
}