I am developing a servo drive for a cnc machine and have run into a problem.
When the motor power is of the encoders and step interrupts work as expected (using the serial port display)
If I turn on motor power (50 vdc), then the encoders trigger and add up real fast.
I am using PWM pins 9 and 10 with a modified frequency and resoultion.
I use INT0 for channelA encoder and INT1 for the step count
Any ideas on why my PWM channels are counting?
Also, if I turn on the power with the Arduino plugged in the usb serial connection locks up.
Here is my code, any help would be appreciated
volatile long EncoderCount = 0;
volatile long StepCount = 0;
const int EncoderA=2;
const int Step=3;
const int EncoderB=4;
const int Direction=5;
const int PulseA=9;
const int PulseB=10;
const double MaxSumOfError=256;
long Error=0;
long SumOfError=0;
double P=0;
double I=0;
double D=0;
double PID=0;
double Kp=0;
double Ki=0;
double Kd=0;
double IMax=1000;
long PreviousEncoderCount=0;
ISR(INT0_vect)
{
if (PIND &_BV(4) )
{
EncoderCount++;
}
else
{
EncoderCount--;
}
}
ISR(INT1_vect)
{
if (PIND &_BV(5) )
{
StepCount++;
}
else
{
StepCount--;
}
}
void setup()
{
pinMode (EncoderA,INPUT);
pinMode (EncoderB,INPUT);
pinMode (Step,INPUT);
pinMode (Direction,INPUT);
pinMode (PulseA,OUTPUT);
pinMode (PulseB,OUTPUT);
EICRA = 15;
EIMSK = 3;
TCCR1B = TCCR1B & 0b11111000 |0x01; // change frequency
//*/*********************************************************************************/
// Set pwm resolution to mode 7 (10 bit)
//**********************************************************************************/
TCCR1B &= ~(1 << WGM13); // Timer B clear bit 4
TCCR1B |= (1 << WGM12); // set bit 3
TCCR1A |= (1 << WGM11); // Timer A set bit 1
TCCR1A |= (1 << WGM10); // set bit 0
analogWrite(PulseA,512);
invertAnalogWrite(PulseB,512);
delay(500);
Serial.begin(38400);
}
void loop()
{
// Error is the difference in stepper counter and encoder count interrupts
Error = StepCount - EncoderCount;
// proportional
P =Kp * Error;
// Integral
SumOfError += Error; // Add up the error
IMax = MaxSumOfError /Ki; //Make Sure that the sum of the error does not get too large
if (SumOfError >IMax)
SumOfError= IMax;
else if (SumOfError < -IMax)
SumOfError = -IMax;
I = Ki * SumOfError;
//derivative
D = (Kd * (StepCount - PreviousEncoderCount));
PreviousEncoderCount= EncoderCount;
PID=(P + I - D)+512;
if (PID>1000) PID=1000;
if (PID<24) PID=24;
analogWrite(9,PID);
invertAnalogWrite(10,PID);
delay(100);
Serial.print(EncoderCount);
Serial.print(", ");
Serial.print(StepCount);
Serial.print(", ");
Serial.println(PID);
delay(500);
}
void invertAnalogWrite(int pin, int value)
{
analogWrite(pin, value);
TCCR1A = TCCR1A & ~B00110000; //switch off output B
TCCR1A |= B00110000; //switch on the B output with inverted output
}
