Help with PID via encoder and PWM

Hi all,

I’m trying to use PID to regulate the speed of a DC motor (http://www.pololu.com/catalog/product/1444). The motor has a 64-count quadrature encoder attached. The Arduino is connected to a Sabertooth 2x10 which connects to the motor. The motor is controlled by PWM signals from the Arduino.

My plan is to figure out how fast the motor is turning using the encoder and feed that to the PID library as the input value (http://playground.arduino.cc/Code/PIDLibrary). I am new to programming and I am having trouble figuring out how fast the motor is turning. Here is my code:

#include <PID_v1.h>

 int encodera=52;
 int encoderb=50;
 int encoderpos=0;
 int encoderacur;
 int encoderalast=LOW;
 unsigned long timestart=0;
 unsigned long timeend;

 void setup()
 {
   pinMode(encodera,INPUT);
   pinMode(encoderb,INPUT);
   Serial.begin(9600);

 }
 
 void loop()
 {
if (timestart=0)
{
   timestart=millis();   
}
if (encoderpos<100)
{
   
encoderacur = digitalRead(encodera);
if ((encoderacur==HIGH)&&(encoderalast==LOW))
  {
  if (digitalRead(encoderb)==LOW)
  {
    encoderpos--;
  }
  else
  {
    encoderpos++;
  }

 //Serial.print("-{");
 //Serial.print(encoderpos);
 //Serial.print("}");
  encoderalast=HIGH;
  }
encoderalast=encoderacur;
}
else
{
  timeend=millis();
  encoderpos=0;
  Serial.print("-[speed rpm ");
  Serial.print(30000/(timeend-timestart));
  Serial.print("]");
  timestart=0;
}
 }

Specifically, the terminal window shows that my motor starts at 23 RPM and gradually slows to 0 RPM. The motor is definitely not stopping, and it does not sound like it is even slowing down. Can any one tell what my problem might be?

Is there a better way to implement PID than what I am attempting?

If there is code available that does what I’m trying to accomplish, I couldn’t find it, but it would be great if you could link me to it.

Any help is greatly appreciated and I would be glad to supply any details I may have missed.

Thank you

Your program might be too slow to read the encoder. The encoder seems to produce 2.5Khz which is pretty fast for an arduino without any speed precautions. On the other hand the program is simple and it looks like it should work. I would suggest to get rid of the second encoder input since you know the direction or run it at a slower speed to see if it is too slow. You should know a serial print can slow down the loop very quickly. Probably add a timer for the loop to see if it is too slow.
I just found a mistake: if (timestart=0) should be "=="; Good luck with the PID, I just finished with a pid myself.

Thanks for the suggestions. I have since changed the encoder code to be based on interrupts and I believe it is calculating speed correctly now. My new code is as follows:

//Encoder code adapted from Chris J. Kiick http://forum.arduino.cc/index.php/topic,158385.0.html
//PID code adapted from Brett Beauregard http://playground.arduino.cc/Code/PIDLibrary
//Alex Wolf

#include <PID_v1.h>

double Setpoint=25;
double Input;
double Output;

float Kp=2;
float Ki=0;
float Kd=0;
int duration=100;

PID myPID(&Input, &Output, &Setpoint,Kp,Ki,Kd, DIRECT);

#define encoder0PinA  3
#define encoder0PinB  2

volatile long encoder0Pos=0;
long newposition;
long oldposition = 0;
unsigned long newtime;
unsigned long oldtime = 0;
int vel;

void setup()
{
  pinMode(encoder0PinA, INPUT);
  digitalWrite(encoder0PinA, HIGH);       // turn on pullup resistor
  pinMode(encoder0PinB, INPUT);
  digitalWrite(encoder0PinB, HIGH);       // turn on pullup resistor
  attachInterrupt(0, doEncoder, RISING);  // encoDER ON PIN 2
  Serial.begin (9600);

  //turn the PID on
  myPID.SetMode(AUTOMATIC);
}

void loop()
{
  newposition = encoder0Pos;
 newtime = millis();
 vel = (newposition-oldposition) * 18.75 /(newtime-oldtime);
  Serial.println (vel);
 delay(duration);
  Input = vel;
  myPID.Compute();
  analogWrite(13,Output);
  oldposition = newposition;
  oldtime = newtime;
}

void doEncoder()
{
  if (digitalRead(encoder0PinA) == digitalRead(encoder0PinB)) {
    encoder0Pos++;
  } else {
    encoder0Pos--;
  }
}

Now, when I set setpoint=25, I expected the algorithm to calculate the PWM values for the motor to achieve that speed on the encoder. instead, the program just sends PWM 25 to the motor on pin 13.

Any suggestions?

Could it be because you turned on the pid in setup before you set Input / Output?