I have an Arduino Leonardo board with an Arduino motor shield connected on top, I have a potentimeter in pin A4 which will be acting as the setpoint for my PID control and a servo connected to pin A5 which will be moving by twisting the potentiometer(A4) the motor for the servo is connected into A on the motor shield, I also have a 9v battery connected to the board for extra power as the 5v input from the usb lead is not enough power... can you please have a look at my code and tell me where I've gone wrong. I have no idea how to set up a PID control loop as you can probably see. Regards.
#include <PID_v1.h>
#include <stdio.h>
double Error, Time, PreviousTime, PreviousError, LastPosition, Proportional, Integral, Derivative, DutyCycle, ActualPosition;
int Drive, ScaleFactor, IntThresh, Motor, P, I, D, dt;
int Pot = A4;
int Servo = A5;
double Kp = 0.5;
double Ki = 1.55;
double Kd = -0.01;
double SetPoint, Input, Output;
PID myPID(&Input, &Output, &SetPoint, Kp, Ki, Kd, DIRECT);
void setup()
{
Serial.begin(9600);
myPID.SetMode(MANUAL);
SetPoint = map(analogRead(A4), 0, 1024, 0, 255);
}
void loop()
{
SetPoint = map(analogRead(A4), 0, 1024, 0, 255);
ActualPosition = analogRead(A5);
myPID.Compute();
analogWrite(A5, OUTPUT);
Time = millis();
ControlTheory();
Error = SetPoint - ActualPosition;
P = Error*Kp;
I = Integral*Ki;
D = (LastPosition - ActualPosition)*Kd;
Drive = P + I + D;
if (Drive < 0)
{
analogWrite(A5, LOW);
}
else
{
analogWrite(A5, HIGH);
}
LastPosition = ActualPosition;
printPIDResults();
}
int ControlTheory()
{
Error = SetPoint - ActualPosition;
Proportional = Error*Kp;
Integral = Integral+(Error*dt);
Derivative = (Error-PreviousError)/dt;
Drive = (Proportional)+(Integral*Ki)+(Derivative*Kd);
PreviousError = Error;
return Proportional + Integral + Derivative;
}
void printPIDResults()
{
Serial.print("PID formula (P + I + D): ");
Serial.print(Proportional, 2);
Serial.print(" + ");
Serial.print(Integral, 2);
Serial.print(" + ");
Serial.println(Derivative, 2);
}