Here is my motor control PID control so far. It is not complete, but it does function. The PID parameters still need adjusted.
I input a step and direction with Turbocnc via a parallel port, then turn the motor until the encoder and Turbocnc pulse count matches,
#define OFF 511
volatile long EncoderCount = 0;
volatile long StepCount = 0;
long Error=0;
long SumOfError=0;
long PreviousEncoderCount=0;
double P=0;
double I=0;
double D=0;
double PID=0;
double Kp=6.0;
double Ki=0.0037;
double Kd=2;
ISR(INT0_vect)
{
if (PIND &_BV(5) ) // get state of PD5 ENCODER B
{
EncoderCount--;
}
else
{
EncoderCount++;
}
}
ISR(INT1_vect)
{
if (PIND &_BV(4) ) // get state of PD4 DIRECTION
{
StepCount++;
}
else
{
StepCount--;
}
}
void setup()
{
// set used pins as input/output
DDRD&=~_BV(2); //set PD2 pin 4 to zero as input ENCODER A (2)
DDRD&=~_BV(3); //set PD3 pin 5 to zero as input STEP (3)
DDRD&=~_BV(4); //set PD4 pin 6 to zero as input DIRECTION (4)
DDRD&=~_BV(5); //set PD5 pin 11 to zero as input ENCODER B (5)
DDRD&=~_BV(6); //set PD6 pin 12 to zero as input ENCODER Z (6)
DDRD|=_BV(7); // set PD7 pin 13 to one as output Q1 (7)
DDRB|=_BV(0); // set PB0 pin 14 to one as output Q2 (8)
DDRB|=_BV(1); // set PB1 pin 15 as PWM output Q3 (9)
DDRB|=_BV(2); // set PB2 pin 16 as PWM output Q4 (10)
// set unused open pins as output
DDRB|=_BV(3); //set PC3 pin 17 to one as output (11)
DDRB|=_BV(4); //set PC4 pin 18 to one as output (12)
DDRB|=_BV(5); //set PC5 pin 19 to one as output (13)
DDRC|=_BV(0); //set PC0 pin 23 to one as output (A0)
DDRC|=_BV(1); //set PC1 pin 24 to one as output (A1)
DDRC|=_BV(2); //set PC2 pin 25 to one as output (A2)
DDRC|=_BV(3); //set PC3 pin 26 to one as output (A3)
DDRC|=_BV(4); //set PC4 pin 27 to one as output (A4)
DDRC|=_BV(5); //set PC5 pin 28 to one as output (A5)
DDRD|=_BV(0); //set PC3 pin 17 to one as output RX (0)
DDRD|=_BV(1); //set PC4 pin 18 to one as output TX (1)
// Set timer1 for 9 bit PWM at 4kHz,
TCCR1A =162; // COM1A1=1, COM1B1=1, WGM11=1
TCCR1B=10; // WGM12=1, CS11=1
OCR1A =OFF; // Set PWM Pin 9
OCR1B =OFF; // Set PWM Pin 10
EICRA = 15; //The rising edge of INT0 and INT1 generates an interrupt request
EIMSK = 3; //enable both interrupt INT0 and INT1
}
void loop()
{
// Error is the difference in stepper counter and encoder count interrupts
// proportional
Error = StepCount - EncoderCount;
P =Kp * Error;
// Integral
SumOfError += Error; // Add up the error
I = Ki * SumOfError;
//derivative
D = (Kd * (StepCount - PreviousEncoderCount));
PreviousEncoderCount= EncoderCount;
PID= P + I - D;
if(PID>OFF)
{
PID=OFF;
}
else if (PID<-OFF)
{
PID=-OFF;
}
if (PID>0)
{
PORTD&= ~_BV(7); // Q1 low
OCR1B = OFF; // Q4 off
PORTB|= _BV(0); // Q2 high
OCR1A = OFF-PID; // Set Q3
}
else if (PID<0)
{
PORTB&= ~_BV(0); // Q2 low
OCR1A = OFF; // Q3 off
PORTD|= _BV(7); // Q1 high
OCR1B = OFF+PID; // Set Q4
}
else
{
//Brake
PORTD&= ~_BV(7); // Q1 low
PORTB&= ~_BV(0); // Q2 low
OCR1A = OFF; // Q3 off
OCR1B = OFF; // Q4 off
}
delayMicroseconds(100);
}