Pages: [1]   Go Down
Author Topic: Encoders changing and serial crashing on servo  (Read 497 times)
0 Members and 1 Guest are viewing this topic.
0
Offline Offline
Jr. Member
**
Karma: 0
Posts: 64
Arduino rocks
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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:
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
}









Logged

Seattle, WA USA
Offline Offline
Brattain Member
*****
Karma: 601
Posts: 48543
Seattle, WA USA
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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

0
Offline Offline
Jr. Member
**
Karma: 0
Posts: 64
Arduino rocks
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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/
« Last Edit: July 25, 2010, 12:15:42 pm by rlwoodjr » Logged

Pages: [1]   Go Up
Jump to: