I am using PinChangedInt software interrupt because I don't have enough hard interrupt pins on my Nano to measure the duration of pulses from two slow wheel encoder disks to get speed feedbacks for speed control PID loops. My encoder pulses are very steady and look like they have the same pulse duration for a constant speed. The wheels are slow and I get about from 3 to 10 pulses/sec from the encoders depending on what speed I am running..
The problem is the pulse measurement variable (elapsedR) from the interrupt routines is all over the place (sometimes zero, sometimes even negative). What is wrong? I need an expert on using the PinChangedInt library. Here is my code:
// Nano drive control**************************************************************************
#undef NO_PORTB_PINCHANGES // to indicate that port b will not be used for pin change interrupts
#undef NO_PORTC_PINCHANGES // to indicate that port c will not be used for pin change interrupts
#include <PinChangeInt.h>
#include <PinChangeIntConfig.h>
#include <PID_v1.h>
//Define Variables we'll be connecting to (append L=left wheel, R=right wheel)
double act_speedL, pid_speedL;
double act_speedR, pid_speedR;
double set_speedL = 30;//set = -30 //for testing
double set_speedR = 20;//30;
double KpL = 0.50; // PID P Gain
double KiL = 0.00; // PID I Gain
double KdL = 0.00; // PID D gain
double KpR = 0.50; // PID P Gain
double KiR = 0.00; // PID I gain
double KdR = 0.00; // PID D gain
//const int LOOPTIME = 100;//200 // PID update timer
/*
//PID controller constants
float KP = 2.25 ; //position multiplier (gain)
float KI = .25; // Intergral multiplier (gain)
float KD = 1.0; // derivative multiplier (gain)
*/
//Specify the links and initial tuning parameters
PID myPIDL(&act_speedL, &pid_speedL, &set_speedL,KpL,KiL,KdL, DIRECT);
PID myPIDR(&act_speedR, &pid_speedR, &set_speedR,KpR,KiR,KdR, DIRECT);
int x = 0;
int IN_A1 = 3; // input RPWM (forward L motor)
int IN_A2 = 10; // input LPWM (reverse L motor)
int IN_B1 = 9; // input RPWM (forward R motor)
int IN_B2 = 11; // input LPWM (reverse R motor)
const int encoderLpin = 4; // the left encoder pin
const int encoderRpin = 5; // the left encoder pin
const int ledPin = 13; // the number of the LED pin
int encoderL = 0; // variable for reading the encoder status
int encoderR = 0; // variable for reading the encoder status
volatile int StartL;
volatile int elapsedL;
volatile int StartR;
volatile int elapsedR;
unsigned long lastMilli = 0; // for PID timer
void setup()
{
// put your setup code here, to run once:
Serial.begin(9600);
Serial.flush();
PCintPort::attachInterrupt(encoderLpin, speedCalcL, RISING); // attach a PinChange Interrupt to our pin
PCintPort::attachInterrupt(encoderRpin, speedCalcR, RISING); // attach a PinChange Interrupt to our pin
StartL=micros();
StartR=micros();
/** motor drive PWM pin configurate */
pinMode(IN_A1, OUTPUT);
pinMode(IN_A2, OUTPUT);
pinMode(IN_B1, OUTPUT);
pinMode(IN_B2, OUTPUT);
//motors stop
digitalWrite(IN_A1, HIGH);
digitalWrite(IN_A2, HIGH);
digitalWrite(IN_B1, HIGH);
digitalWrite(IN_B2, HIGH);
// initialize the LED pin as an output:
pinMode(ledPin, OUTPUT);
// initialize the encoder pins as input:
pinMode(encoderLpin, INPUT); //set the pin to input
pinMode(encoderLpin, INPUT_PULLUP); // need pullup for the TCRT5000L's
pinMode(encoderRpin, INPUT); //set the pin to input
pinMode(encoderRpin, INPUT_PULLUP);
// increase frequency of PWM on pins 11 & 12 (TCCR1B) and pins 3 & 5 (TCCR3B)
int prescalerVal = 0x07; // create a variable called prescalerVal and set it equal to the binary number "00000111"
TCCR1B &= ~prescalerVal; //AND the value in TCCR1B with binary number "11111000"
TCCR2B &= ~prescalerVal; //AND the value in TCCR2B with binary number "11111000"
prescalerVal = 0x01; //set prescalerVal equal to binary number "00000010"
TCCR1B |= prescalerVal; //OR the value in TCCR1B with binary number "00000001"
TCCR2B |= prescalerVal; //OR the value in TCCR2B with binary number "00000001"
//turn the PIDs on
myPIDL.SetOutputLimits(0, 100);
myPIDR.SetOutputLimits(0, 100);
myPIDL.SetMode(AUTOMATIC);
myPIDR.SetMode(AUTOMATIC);
}
void speedCalcL() // ISR to read the elapsed encoder time and put it into elapsedL
{
elapsedL = micros() - StartL;
StartL = micros();
}
void speedCalcR() // ISR to read the elapsed encoder time and put it into elapsedR
{
elapsedR = micros() - StartR;
StartR = micros();
}
void loop()
{
x++; if (x > 100) x=0; // do prints every x cycles
// put your main code here, to run repeatedly:
// if there's any serial available, read the set speeds:
// if (Serial.available()) {
// look for the next valid integer in the incoming serial stream:
// int set_speedL = Serial.parseInt();
// do it again:
// int set_speedR = Serial.parseInt();
// look for the newline. That's the end of your sentence:
// if (Serial.read() == '\n') {
//control motors here:
// if (set_speedL == -1) set_speedL = 0;
// if (set_speedR == -1) set_speedR = 0;
// read the state of the encoder for LED indicator:
encoderR = digitalRead(encoderRpin);
if (encoderR == HIGH) {
digitalWrite(ledPin, HIGH);
}
else {
digitalWrite(ledPin, LOW);
}
// calibration factor for encoder pulse duration to speed equivalent to set_speed
if (set_speedL > 0 ) act_speedL= 25500/elapsedL;
if (set_speedL < 0 ) act_speedL= - 25500/elapsedL; //act = -25
if (set_speedR > 0 ) act_speedR= 21500/elapsedR;
if (set_speedR < 0 ) act_speedR= - 25500/elapsedR; //act = -25
//do pid calculation:
// if((millis()-lastMilli) >= LOOPTIME) { // do pid calculation at set intervals
// lastMilli = millis();
myPIDL.Compute();
myPIDR.Compute();
// }
//*******Plot data for MegunoLink***************************************************************
if (x == 0) {
// sendPlotData("elapsedL", elapsedL);
// sendPlotData("set_speedL", set_speedL);
// sendPlotData("act_speedL", act_speedL);
// sendPlotData("pid_speedL", pid_speedL);
// sendPlotData("encoderR", encoderR);
sendPlotData("elapsedR", elapsedR);
// sendPlotData("set_speedR", set_speedR);
// sendPlotData("act_speedR", act_speedR);
// sendPlotData("pid_speedR", pid_speedR);
}
// send pid speeds to motor drive:
if(set_speedL >= 0){ // if set speed is forward
analogWrite(IN_A1,(int(pid_speedL)));
analogWrite(IN_A2,0);
}else{ // if set speed is reverse
analogWrite(IN_A1,0);
analogWrite(IN_A2,-(~(int(pid_speedL))));
}
if(set_speedR >= 0 ){
analogWrite(IN_B1,(int(set_speedR)));
analogWrite(IN_B2,0);
}else{
analogWrite(IN_B1,0);
analogWrite(IN_B2,-(~(int(set_speedR))));
}
}
//*********format data for MegunoLink ******************
void sendPlotData(String seriesName, float data)
{
Serial.print("{"); // sendPlotData headingDegrees
Serial.print(seriesName);
Serial.print(",T,");
Serial.print(data);
Serial.println("}");
}
//********* end *****************************************