Go Down

Topic: Problem: PID speed with wheel encoders and PinChangeInt (Read 2283 times) previous topic - next topic


Jul 07, 2013, 09:40 pm Last Edit: Jul 07, 2013, 09:44 pm by warren631 Reason: 1
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:
Code: [Select]
// 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:

 PCintPort::attachInterrupt(encoderLpin, speedCalcL, RISING); // attach a PinChange Interrupt to our pin
 PCintPort::attachInterrupt(encoderRpin, speedCalcR, RISING); // attach a PinChange Interrupt to our pin

 /** 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);


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();
//  }  

  //*******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
}else{                                      // if set speed is reverse

if(set_speedR >= 0 ){                        

//*********format data for MegunoLink ******************
void sendPlotData(String seriesName, float data)
 Serial.print("{");  // sendPlotData headingDegrees

//********* end *****************************************


Your StartL and StartR variables are ints.

micros() has a value between 0 and 70 (minutes till reset) x 60 (seconds per minute) x 1M (microseconds per second) = 4.2 billion.

The elapsed time probably fits in an int, but your starting and subsequent update times will most likely not.

unsigned long is the type used in the micros() example.
Windows serial port monitor: Tellurium | Arduino serial port debugging library: DBG | Cusom LCD char generator | Technical questions will only be answered in forum threads


Besides what Aaron wrote, it appears you are using an old version of PinChangeInt. Try upgrading to the latest, which is 2.21beta.


Jul 09, 2013, 08:33 pm Last Edit: Jul 09, 2013, 08:35 pm by warren631 Reason: 1
Thanks Aron and GG.  I feel so stupid :smiley-red:
I'm using rev 2.17 which is the latest on the website:
I couldn't find rev 2.21.

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!

via Egeo 16
Torino, 10131