Hi, I am trying to write a program that will use the PID library to control the speed of two motors. I have quite a bit of code going on so I will explain what Is happening:
1.Reads the signals from a RC Receiver and then convert them into speed and direction signals for the motors
2.Use interrupts to read the encoders and determine the speed of the motors
3.Use a PID controller to keep the speed of the motors and the input the same.
My issue is that the PID loop does not output any value other than 0 when both the Input and Set point variables are changed.
Any help would be greatly appreciated!
#include <PID_v1.h>
//Define Variables for PID to connect to
double SetpointL, InputL, OutputL;
double SetpointR, InputR, OutputR;
//tuning parameters:
int Kp = 1;
int Ki = 1;
int Kd = 1;
//Specify the links
PID PIDL(&InputL, &OutputL, &SetpointL,Kp,Ki,Kd, DIRECT);
PID PIDR(&InputR, &OutputR, &SetpointR,Kp,Ki,Kd, DIRECT);
int LM;//Left motor vector
int LMD;//Left motor direction
int LMS;//Left motor speed
int RM;//Right motor vector
int RMD;//Right motor direction
int RMS;//Right motor speed
int SPD;//Robot speed
int DIR;//Robot direction
int LSP = 5; //Left speed pin
int LDP = 4; //Left direction pin
int RSP = 6; //Right speed pin
int RDP = 7; //Right direction pin
int CH1;//Throttle (Pin 10)
int CH2;//Rudder (Pin 11)
int CH3;//Manual (Pin 12)
int CH1P = 10;//Channel 1 Pin
int CH2P = 11;//Channel 2 Pin
int CH3P = 12;//Channel 3 Pin
//Connect the encoder pins to variables.
const byte LPA = 2;
const byte LPB = 8;
const byte RPA = 3;
const byte RPB = 9;
byte LPALast;
byte RPALast;
int Lduration;//the number of the pulses
int Rduration;
boolean LDirection;//the rotation direction
boolean RDirection;
void setup() {
Serial.begin(9600);
//Pin modes
pinMode(LSP, OUTPUT);
pinMode(RSP, OUTPUT);
pinMode(LDP, OUTPUT);
pinMode(RDP, OUTPUT);
pinMode(CH1P, INPUT);
pinMode(CH2P, INPUT);
pinMode(CH3P, INPUT);
EncoderInit();//Initialize the two modules
//Initialize the variables we're linked to
InputL = Lduration;
InputR = Rduration;
SetpointL = LM;
SetpointR = RM;
//turn the PID on
PIDL.SetMode(AUTOMATIC);
PIDR.SetMode(AUTOMATIC);
PIDL.SetSampleTime(1);
PIDR.SetSampleTime(1);
}
void loop() {
//Input RC control
CH1 = pulseIn(CH1P, HIGH, 25000);
CH2 = pulseIn(CH2P, HIGH, 25000);
CH3 = pulseIn(CH3P, HIGH, 25000);
//Map RC input
CH1 = map(CH1, 965, 1975, -128, 128);
CH2 = map(CH2, 965, 1975, -128, 128);
//Resolve input
if(CH3 > 1470){
SPD = CH1;
DIR = CH2;
}else{
SPD = 0;
DIR = 0;
}
//Dead Zone
if(abs(SPD) < 10){
SPD = 0;
}
if(abs(DIR) < 10){
DIR = 0;
}
//Resolve motor vectors
LM = (SPD + DIR);
RM = (SPD - DIR);
//Resolve motor direction
if(LM > 0){
LMD = 1;
}else{
LMD = 0;
}
if(RM > 0){
RMD = 1;
}else{
RMD = 0;
}
//Resolve motor magnitude
abs(LM);
abs(RM);
if (LM > 255){
LM = 255;
}
if (RM > 255){
RM = 255;
}
Lduration = map(Lduration, -330, 330, -255, 255);
Rduration = map(Rduration, -330, 330, -255, 255);
InputL = Lduration;
InputR = Rduration;
SetpointL = LM;
SetpointR = RM;
PIDL.Compute();
PIDR.Compute();
OutputL = LMS;
OutputR = RMS;
//DE-BUG:
Serial.print("LM SET: ");
Serial.print(LM);
Serial.print(" LM IN: ");
Serial.print(InputL);
Serial.print(" LM OUT: ");
Serial.println(LMS);
//END
//Write to motors
digitalWrite(LDP,LMD);
digitalWrite(RDP,RMD);
analogWrite(RSP,RMS);
analogWrite(LSP,LMS);
Lduration = 0;
Rduration = 0;
delay(100);
}
void EncoderInit()
{
LDirection = true;//default -> Forward
RDirection = true;
pinMode(LPB,INPUT);
pinMode(RPB,INPUT);
attachInterrupt(0, LwheelSpeed, CHANGE);//int.0
attachInterrupt(1, RwheelSpeed, CHANGE);//int.1
}
void LwheelSpeed()
{
int LLstate = digitalRead(LPA);
if((LPALast == LOW) && LLstate==HIGH)
{
int Lval = digitalRead(LPB);
if(Lval == LOW && LDirection)
{
LDirection = false; //Reverse
}
else if(Lval == HIGH && !LDirection)
{
LDirection = true; //Forward
}
}
LPALast = LLstate;
if(!LDirection){
Lduration++;
}else{
Lduration--;
}
}
void RwheelSpeed()
{
int RLstate = digitalRead(RPA);
if((RPALast == LOW) && RLstate==HIGH)
{
int Rval = digitalRead(RPB);
if(Rval == HIGH && RDirection)
{
RDirection = false; //Reverse
}
else if(Rval == LOW && !RDirection)
{
RDirection = true; //Forward
}
}
RPALast = RLstate;
if(!RDirection){
Rduration++;
}else{
Rduration--;
}
}
