Hello ,
I'm writing a software to controll DC servo with 1000 pps incremental encoder .I need to compare pulse count of controller encoder and motor encoder to keep speed the same . I need speed controll not position . The problem is that when i use this code :
/* read a rotary encoder with interrupts
Encoder hooked up with common to GROUND,
encoder0PinA to pin 2, encoder0PinB to pin 4 (or pin 3 see below)
it doesn't matter which encoder pin you use for A or B
uses Arduino pullups on A & B channel outputs
turning on the pullups saves having to hook up resistors
to the A & B channel outputs
*/
#define encodermotorPinA 2
#define encodermotorPinB 4
#define encodercontrollerPinA 3
#define encodercontrollerPinB 5
#define SpeedPin 10
#define DirectionPin 12
volatile long encodermotorPos = 0;
volatile long encodercontrollerPos = 0;
long target = 0;
//PID controller constants
float KP = 2.25 ; //position multiplier (gain)
float KI = .25; // Intergral multiplier (gain)
float KD = 1.0; // derivative multiplier (gain)
int lastError = 0;
int sumError = 0;
//Integral term min/max (random value and not yet tested/verified)
int iMax = 100;
int iMin = 0;
long previousTarget = 0;
long previousMillis = 0; // will store last time LED was updated
long interval = 5; // interval at which to blink (milliseconds)
void setup() {
pinMode(encodermotorPinA, INPUT);
pinMode(encodermotorPinB, INPUT);
pinMode(encodercontrollerPinA, INPUT);
pinMode(encodercontrollerPinB, INPUT);
pinMode(DirectionPin, OUTPUT);
pinMode(SpeedPin, OUTPUT);
Serial.begin(115200); // Set up serial communication
TCCR1A = 0;
TCCR1B = B01001011; // rising edges, CTC mode, prescaler = 64
OCR1A = 25000; // 25000 x 64 / 16 million -> interrupt every 100ms
TCNT1 = 0; // reset timer1
TIMSK1 = B00000010; // turn on Output Compare A Interrupt
attachInterrupt(1, doEncodercontroller, RISING); // Attaches interrupt to Digi Pin 3
attachInterrupt(0, doEncodermotor, RISING);
}
void loop(){
if (millis() - previousTarget > 1000)
{
previousTarget = millis();
}
target = encodercontrollerPos;
docalc();
}
void docalc() {
if (millis() - previousMillis > interval)
{
previousMillis = millis(); // remember the last time we blinked the LED
long error = encodermotorPos - target ; // find the error term of current position - target
//generalized PID formula
long ms = KP * error + KD * (error - lastError) +KI * (sumError);
lastError = error;
sumError += error;
//scale the sum for the integral term
if(sumError > iMax) {
sumError = iMax;
} else if(sumError < iMin){
sumError = iMin;
}
if(ms > 0){
digitalWrite ( DirectionPin ,HIGH );
}
if(ms < 0){
digitalWrite ( DirectionPin , LOW );
ms = -1 * ms;
}
int motorSpeed = map(ms,-1000,0,0,255);
analogWrite ( SpeedPin, motorSpeed );
}
}
void doEncodermotor(){
encodermotorPos++ ;
Serial.print ( encodermotorPos );
Serial.print ("\n");
}
void doEncodercontroller(){
encodercontrollerPos++ ;
Serial.print ( encodercontrollerPos );
Serial.print ("\n");
}
the motor is not running , the line :
TCCR1A = 0;
disconnects motor .
i need this line to have fast pulse count of doEncodercontroller() like 6Khz.
i cannot do it with DigitalFastWrite becouse it's too slow.
Any help will be very appreciated!!
Tomek