Hello,
I am controlling a motor using a PID controller and a master and slave quadrature encoder on a DUE. When the error between master and slave become greater than 255, my motor cannot read steady states and oscillates. One way I have figured out to solve this is essentially never letting the error go up 255 by incrementing encoder counts by decimals. Doing this makes my control very slow - It will take many revolutions in my master side to complete my slave side.
I tried tuning and changing encoder counts.. I am wondering if people have clues and suggestions?
/*
Project:
Name: Daniel G
Date: 10/15/2012
Company: RRI
*/
/*
EnA | In1 | In2 | Description
-----------------------------------------------
0 | X | X | Free Running Motor Stop
1 | 1 | 0 | Forward
1 | 0 | 1 | ReverslaveEncoder
1 | 1 | 1 | Fast Motor Stop
-----------------------------------------------
CHA: ____EEEE____EEEE____EEEE____EEEE
CHB: EE____EEEE____EEEE____EEEE____EE
When CHA rislaveEncoders check the state of CHB. From here you can determine the
direction of the encoder corresponding to the Master or Slave
*/
#include <stdio.h>
int In1 = 8;
int In2 = 9;
int EnA = 10;
int Master_CHA = 2; // Interrupt INT0 on Pin 2 0
int Master_CHB = 4; //
int Slave_CHA = 3; // Interrupt INT1 on Pin 3 1
int Slave_CHB = 6; //
int Master_CHB_val = 0;
int Slave_CHB_val = 0;
volatile float masterEncoder = 0;
volatile float slaveEncoder = 0;
// Control Signal and Error
float u = 0;
float err = 0;
// PID Control
int Kp = 10;
double Ki = .008;
double Kd = 0.2;
// Error Variables
float err_old = 0;
float err_intgrl = 0;
float err_dot = 0;
int max_int = 255;
int max_der = 255;
void setup()
{
Serial.begin(115200);
// slaveEncodertting Motor Pins
pinMode(In1, OUTPUT); // Setting In1 (direction indicator)
pinMode(In2, OUTPUT); // Setting In2 (direction indicator)
pinMode(EnA, OUTPUT); // Setting PWM output
// Set Encoder Pins & Interrupt
pinMode(Master_CHB, INPUT);
pinMode(Slave_CHB, INPUT);
attachInterrupt(Master_CHA, Master_checkDirection, RISING); // CHA causes interrupt service routine
attachInterrupt(Slave_CHA, Slave_checkDirection, RISING); // CHA causes interrupt service routine
// PWMC_ConfigureClocks(4000 * PWM_MAX_DUTY_CYCLE , 0, VARIANT_MCK);
// TCCR1B = TCCR1B & 0b11111000 | 0x02; // PWM frequency changed from 500Hz to 3.922KHz - L298 typically can support 25KHz.
}
// ISR for Master (Controller)
void Master_checkDirection()
{
Master_CHB_val = digitalRead(Master_CHB); // When CHA RAISES check CHB
if (Master_CHB_val == 0) masterEncoder += 0.1; // Increment Master count for a direction
else masterEncoder -= 0.1; // Decrement Master count for the other direction
}
// ISR for Slave (Motor)
void Slave_checkDirection()
{
Slave_CHB_val = digitalRead(Slave_CHB); // When CHA RAISES check CHB
if (Slave_CHB_val == 0) slaveEncoder += 0.1; // Increment Slave count for a direction
else slaveEncoder -= 0.1; // Decrement Slave count for the other direction
}
// Function to Set Direction of PWM
void setDirection(boolean In1_State, boolean In2_State)
{
digitalWrite(In1, In1_State);
digitalWrite(In2, In2_State);
}
void loop()
{
err = slaveEncoder - masterEncoder;
err_dot = err - err_old;
err_dot = constrain(err_dot, -max_der, max_der);
err_intgrl = err + err_intgrl;
err_intgrl = constrain(err_intgrl, -max_int, max_int);
u = float(Kp*err + Kd*err_dot + Ki*err_intgrl);
u = constrain(u, -255, 255);
if (u < 0) {setDirection(LOW, HIGH); u = -u;}
else setDirection(HIGH, LOW);
analogWrite(EnA, u); // Write PWM value as the encoder count of the Master (controller)
err = err_old;
Serial.print(masterEncoder);
Serial.print("|");
Serial.print(slaveEncoder);
Serial.print("|");
Serial.println(u);
}