Go Down

Topic: Encoders changing and serial crashing on servo (Read 542 times) previous topic - next topic

rlwoodjr

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

Code: [Select]
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
}











PaulS

Quote
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.

Since both of the anomalies occur when the external power source is used, I don't suspect that it's a software issue. We'd need to know more about the external power, the servos and encoders, and how they are wired up.

rlwoodjr

#2
Jul 25, 2010, 07:05 pm Last Edit: Jul 25, 2010, 07:15 pm by rlwoodjr Reason: 1
I would add a schematic image, but I dont seem to have a button to do so.  :-[

I am sending the output of pins 9 and 10 of the arduino to a pair of FAN73832's.

I added my schematic here:
http://img690.imageshack.us/f/fan73832.png/

Go Up