Go Down

Topic: Encoders changing and serial crashing on servo (Read 646 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
 


Please enter a valid email to subscribe

Confirm your email address

We need to confirm your email address.
To complete the subscription, please click the link in the email we just sent you.

Thank you for subscribing!

Arduino
via Egeo 16
Torino, 10131
Italy