Pages: [1]   Go Down
Author Topic: Problem: PID speed with wheel encoders and PinChangeInt  (Read 846 times)
0 Members and 1 Guest are viewing this topic.
Offline Offline
Full Member
***
Karma: 1
Posts: 109
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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:
// 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 *****************************************
« Last Edit: July 07, 2013, 02:44:15 pm by warren631 » Logged

Melbourne, Australia
Offline Offline
God Member
*****
Karma: 8
Posts: 567
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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

Windows serial port monitor: Tellurium | Arduino serial port debugging library: DBG | Cusom LCD char generator | Technical questions will only be answered in forum threads

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

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

Offline Offline
Full Member
***
Karma: 1
Posts: 109
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Thanks Aron and GG.  I feel so stupid smiley-red
I'm using rev 2.17 which is the latest on the website:
http://code.google.com/p/arduino-pinchangeint/downloads/list
I couldn't find rev 2.21.
« Last Edit: July 09, 2013, 01:35:12 pm by warren631 » Logged

Pages: [1]   Go Up
Jump to: