PID Motor position control, output issues

I'm am trying to use the PID library to control the position on a motor. So as the motor position changes decrease the speed till it reaches 0 at the target position. The PID output is always 255 though. No matter what turnings I use its always 255 right from the jump and never decreases. I know the position is changing because I can see it in the serial monitor. Right now the turnings are set to some random settings.

#include <PID_v1.h>

#define encoderPinA 18
#define encoderPinB 19

int enablePin = 5; //h bridge enable pin 1 for left motor
int inputPin1 = 4; // h bridge input 1 for left mootor

double Kp = 0, Ki = 5, Kd = 0;
double position, speed, setPoint;
volatile double intPosition;

PID myPid(&position, &speed, &setPoint, Kp, Ki, Kd, AUTOMATIC);

void setup() 
{
	Serial.begin(115200);
	speed = 0;
	setPoint = 87;
	position =0;
	intPosition = 0;
	 
	myPid.SetMode(AUTOMATIC);
	myPid.SetSampleTime(1);

	pinMode(enablePin, OUTPUT);
	pinMode(inputPin1, OUTPUT);
	digitalWrite(inputPin1, HIGH);

	attachInterrupt(digitalPinToInterrupt(19), leftISR, CHANGE);
}


void loop() 
{
	uint8_t oldSREG = SREG;                           // Store interrupt status register

	cli();
	position = intPosition;
	SREG = oldSREG;

	myPid.Compute();

	if (position < setPoint)
	{
		setSpeed(speed);
	}
	Serial.print("Speed: ");
	Serial.println(speed);

}

void leftISR()
{
	if (digitalRead(encoderPinA) == LOW)
	{
		if (digitalRead(encoderPinB) == HIGH)
		{
			intPosition++;
			Serial.print("POSISTION: ");
			Serial.println(intPosition);
		}
	}

}

void setSpeed(float speed)
{
	analogWrite(enablePin, speed);
}

You may be interested in my explorations of the PID concept here. I did eventually get it working.

...R

From that link provided, did you end up using ZHomeSlices PID_v2 library? Seems to get better performance than V1, but I can't get it to work.

The output of the PID is a signed quantity and must control direction and speed accordingly. Ideally you'd
use a bridge in sychronous rectification mode, rather than a decay mode, so that active braking happens.

AgentNoise:
From that link provided, did you end up using ZHomeSlices PID_v2 library? Seems to get better performance than V1, but I can't get it to work.

No. I use the code I posted in Reply #72 of that Thread. I can't recall how close it is to @zhomeslice's version.

By avoiding the use of floating point maths the time to compute the PID is very much reduced.

"I can't get it to work" does not provide any clue from which we may be able to help.

...R

Ideally you'd use a bridge in sychronous rectification mode, rather than a decay mode

I know some of those words....

"I can't get it to work" does not provide any clue from which we may be able to help.

Fair. It always says that the functions for the PID object are undefined. The .h file is there and the object is created OK.

I have actually gotten it very close to functioning at an acceptable level. The main issue I am running into now is dropping below usable voltage before the motor reaches position. The encoder has 332 steps in a rotation, if I am reading the rising and falling of both encoders. Anything less than 249(75%) it stops early and you get a lot of motor whine. I am thinking of using variable tuning bases on percent of a full rotation unless there are some suggestions for a better solution.

#include <Encoder.h>
#include <PID_v1.h>

int enablePin = 6; //h bridge enable pin 1 for right motor
int inputPin1 = 8; // h bridge input 1 for right mootor
int inputPin2 = 9; // h bridge input 2 for right motor

double Kp = 1.2, Ki = 0.005, Kd =3.125;
double inputPosition, outSpeed, setPoint;
volatile double intPosition;

Encoder myEnc(20, 21);
PID myPid(&inputPosition, &outSpeed, &setPoint, Kp, Ki, Kd, DIRECT);


void setup()
{
  Serial.begin(115200);

  outSpeed = 0;
  setPoint = 166;
  inputPosition = 0;

  int enablePin = 5; //h bridge enable pin 1 for right motor
  int input1 = 4; // h bridge input 1 for right mootor
  int input2 = 7; // h bridge input 2 for right motor

  pinMode(enablePin, OUTPUT);
  pinMode(inputPin1, OUTPUT);
  pinMode(inputPin2, OUTPUT);

  digitalWrite(inputPin1, HIGH);
  digitalWrite(inputPin2, LOW);

  myPid.SetMode(AUTOMATIC);
  myPid.SetSampleTime(1);
}

void loop()
{
  uint8_t oldSREG = SREG;                           // Store interrupt status register

  cli();
  inputPosition = myEnc.read();
  SREG = oldSREG;

  myPid.Compute();

  setSpeed(outSpeed);
  Serial.print("Speed: ");
  Serial.println(outSpeed);
}

void setSpeed(float speed)
{
  analogWrite(enablePin, speed);
}

AgentNoise:
Fair. It always says that the functions for the PID object are undefined.

If you are getting an error message from the compiler then you need to post the actual text of the message.

I presume the code in Reply #5 is the exact code that causes the error?

...R

Robin2:
If you are getting an error message from the compiler then you need to post the actual text of the message.

I presume the code in Reply #5 is the exact code that causes the error?

...R

Ehhh, I had mostly given up on the PID_V2. The code in post #5 is for my issue with the voltage. Code below and message are what I get with PID_v2. Its his test code with just the pwm pin changed

// This version of Tacometer auto adjusts for difference in pulses
#include <PID_v2.h> // uses my new version of PID_v1

//Define Variables we'll be connecting to
double Setpoint, Input, Output;

//Specify the links and initial tuning parameters
PID myPID(&Input, &Output, &Setpoint, .05, .16, .001, DIRECT);

#define PWMpin  9

int SampleDuration = 30; // in Milliseconds
volatile unsigned long timeX = 1;
int PulsesPerRevolution = 400;

volatile int Counts = 10;
double PulsesPerMinute;
volatile unsigned long LastTime;
volatile int PulseCtrX;
int PulseCtr;
unsigned long Counter;
unsigned long Time;
/*
  //Self Test
  int RPM = 1;
  double TextRPM = 1;
*/
void setup() {
  // note that 1666666.67 = (60 seonds * 1000000 microseconds)microseconds in a minute / (36 / 9) pulses in 1 revolution
  PulsesPerMinute = (60 * 1000000) / (PulsesPerRevolution / Counts);
  Setpoint = 3100;
  pinMode(2, INPUT_PULLUP);
  pinMode(3, OUTPUT);
  pinMode(4, OUTPUT);
  digitalWrite(4, HIGH);
  Serial.begin(115200);
  Serial.println("aTest Tachometer");
  delay(1000);
  //Digital Pin 2 Set As An Interrupt for tacho.
  attachInterrupt(0, sensorInterrupt, FALLING);

  myPID.SetSampleTime(1);
  myPID.SetOutputLimits(0, 255);
  PulseCtr = 0;
  myPID.SetMode(AUTOMATIC);
  myPID.PrimeIntegral(100);
  myPID.Compute();
  analogWrite(PWMpin, Output);
  delay(100);
  myPID.Compute();


}

void loop() {
  readRpm();
  //  SelfTest();
  static unsigned long SpamTimer;
  if ((unsigned long)(millis() - SpamTimer) >= 15000) {
    SpamTimer = millis();
    Setpoint = (Setpoint == 3100) ? 7100 : 3100;
  }
}
/*
  void SelfTest(){
  static unsigned long TestTimer;
  static bool done = false;
  static unsigned long TestTime;
  if(done)return;
  if ((unsigned long)(micros() - TestTimer) >= TestTime) { // rpm test
    TestTimer += TestTime;// we want an exact duration for our tests
    TestTime = (60000000 / (PulsesPerRevolution * RPM));// we are one pulse behind so calculate next delay
    sensorInterrupt();
    if (RPM <= 850) {
      TextRPM += .04;
      RPM = (int) TextRPM;
    }
    else done = true;
  }

  }
*/

// New version of sensorInterrupt
void sensorInterrupt()
{
  static int Ctr;
  unsigned long Time;
  Ctr++;
  if (Ctr >= Counts) { // so we are taking an average of "Counts" readings to use in our calculations
    Time = micros();
    timeX += (Time - LastTime); // this time is accumulative ovrer those "Counts" readings
    LastTime = Time;
    PulseCtrX ++; // will usually be 1 unless something else delays the sample
    Ctr = 0;
  }
}

void sensorInterruptX()
{
  static int Ctr;
  unsigned long Time;
  Ctr++;
  if (Ctr >= Counts) { // so we are taking an average of "Counts" readings to use in our calculations
    Time = micros();
    timeX += (Time - LastTime); // this time is accumulative ovrer those "Counts" readings
    LastTime = Time;
    PulseCtrX ++; // will usually be 1 unless something else delays the sample
    Ctr = 0;
  }
}



void readRpm()
{
  if (!PulseCtrX) return; // << Added lets not stop interrupts unless we know we are ready (keep other code happy).
  cli ();         // clear interrupts flag
  Time = timeX;   // Make a copy so if an interrupt occurs timeX can be altered and not affect the results.
  timeX = 0;
  PulseCtr = PulseCtrX;
  PulseCtrX = 0;
  sei ();         // set interrupts flag
  if (PulseCtr > 0) {
    Input =  (double) (PulsesPerMinute /  (double)(( (unsigned long)Time ) *  (unsigned long)PulseCtr)); // double has more percision
    //   PulseCtr = 0; // set pulse Ctr to zero
    AverageCapture(Input);
    debug();
    myPID.Compute();
    analogWrite(PWMpin, Output);
    //
    if (Time > ((SampleDuration + 1) * 1000))Counts--;
    if (Time < ((SampleDuration - 1) * 1000))Counts++;
    Counts = constrain(Counts, PulsesPerRevolution * .1, PulsesPerRevolution * 4);
    Time = 0; // set time to zero to wait for the next rpm trigger.
    Counter += PulseCtr;
    PulseCtr = 0; // set pulse Ctr to zero
    PulsesPerMinute = (60.0 * 1000000.0) / (double)((double)PulsesPerRevolution / (double)Counts);
  }
}
float AvgArray[100];
int Readings = 0;
void AverageCapture(float in) {
  static int Position = 0;
  AvgArray[Position] = in;
  Position++;
  Readings++;
  Readings = min (100, Readings); // 100 readings 0-99;
  if (Position >= 100)Position = 0;//99 spots
}
float AverageValue() {
  float Total = 0;
  float Average;
  if (!Readings)return (0.0);
  for (int Position = 0; Position < Readings; Position++) {
    Total += AvgArray[Position];
  }
  Average = Total / Readings;
  return (Average);
}

void debug()
{
  char S[20];
  for (static long QTimer = millis(); (long)( millis() - QTimer ) >= 100; QTimer = millis() ) {  // one line Spam Delay at 100 miliseconds
    Serial.print("Counts: "); Serial.print(Counts );
    //    Serial.print(" Target RPM: ");Serial.print(RPM );
    //    Serial.print(" Counter: "); Serial.print(Counter );
    //    Serial.print(" time: "); Serial.print(Time );
    Serial.print(" DeltaT: "); Serial.print(Time *  PulseCtr);
    Serial.print(" PulseCtr: "); Serial.print(PulseCtr );
    //    Serial.print(" PulsesPerMinute: "); Serial.print(dtostrf(PulsesPerMinute, 6, 1, S));
    Serial.print(" Average: "); Serial.print(dtostrf(AverageValue(), 6, 1, S));

    //   Serial.print(" Setpoint: "); Serial.print(dtostrf(Setpoint, 6, 1, S) );
    Serial.print(" Calculated RPM: "); Serial.print(dtostrf(Input, 6, 1, S));
    Serial.print(" Output: "); Serial.print(dtostrf(Output, 6, 2, S));
    Serial.println();

  }
}

Error

Tachometer_Final_With_Self_Test.cpp.o*: In function setup
Tachometer_Final_With_Self_Test.ino:48: undefined reference to PID  SetSampleTime(int)
Tachometer_Final_With_Self_Test.ino:51: undefined reference to PID  SetOutputLimits(double, double)
Tachometer_Final_With_Self_Test.ino:52: undefined reference to PID  SetMode(int)
Tachometer_Final_With_Self_Test.ino:53: undefined reference to PID  PrimeIntegral(double)
Tachometer_Final_With_Self_Test.ino:54: undefined reference to PID  Compute()
Tachometer_Final_With_Self_Test.ino:57: undefined reference to PID  Compute()
 
Tachometer_Final_With_Self_Test.cpp.o*: In function readRpm()
Tachometer_Final_With_Self_Test.ino:136: undefined reference to PID  Compute()
 
Tachometer_Final_With_Self_Test.cpp.o*: In function __static_initialization_and_destruction_0
Tachometer_Final_With_Self_Test.ino:8: undefined reference to PID  PID(double*, double*, double*, double, double, double, int)
 
collect2.exe*: error: ld returned 1 exit status

Error compiling for board Arduino/Genuino Mega w/ ATmega2560 (Mega 2560)
Debug build failed for project 'Tachometer_Final_With_Self_Test'

That error message does not seem to correspond with the code above it.

The first line with myPID.SetSampleTime(1); is line 42, not 48.

And, are you sure you have the library properly installed?, and the right version of the library?

More importantly,

Ehhh, I had mostly given up on the PID_V2.

if you have you no interest in fixing this problem please let us know.

...R

Robin2:
That error message does not seem to correspond with the code above it.

The first line with myPID.SetSampleTime(1); is line 42, not 48.

And, are you sure you have the library properly installed?, and the right version of the library?

More importantly,if you have you no interest in fixing this problem please let us know.

...R

Thats the exact code and message copied straight from visual studio. But I get the same results in the arduino ide. I have the CPP and H file present in the project folder and added the include. Not sure of any other steps.

I wouldn't mind giving this a go if I can get it working. Though I am also interested in the voltage issue explained in reply #5.

Earlier I had not realized that Reply #5 was a somewhat different question. Now that I have studied it I don't understand this

The main issue I am running into now is dropping below usable voltage before the motor reaches position. The encoder has 332 steps in a rotation, if I am reading the rising and falling of both encoders. Anything less than 249(75%) it stops early and you get a lot of motor whine.

What are you checking to detect the voltage drop?

249 seems to be 75% of 332. I can't understand how you can get less that 332 steps from your encoder every revolution?

And why would the number of encoder steps cause motor whine?

In your code I see cli() but I don't see the follow up sei() to turn the interrupts back on?

And why are fiddling with SREG?
But I am not familiar with the Encoder.h library

If you want the PID function to control the speed I don't understand why you are giving it the encoder count?

...R

Robin2:
Earlier I had not realized that Reply #5 was a somewhat different question. Now that I have studied it I don't understand thisWhat are you checking to detect the voltage drop?

249 seems to be 75% of 332. I can't understand how you can get less that 332 steps from your encoder every revolution?

And why would the number of encoder steps cause motor whine?

In your code I see cli() but I don't see the follow up sei() to turn the interrupts back on?

And why are fiddling with SREG?
But I am not familiar with the Encoder.h library

If you want the PID function to control the speed I don't understand why you are giving it the encoder count?

...R

There are 332 steps in the rotation but what if I want to move less than a full rotation. What I have been doing is as the steps near the setpoint the PWN(speed) begins to slow, until it reaches 0 at the desired setpoint. The problem is, the motor does not turn when the PWM is too low and you just get a lot of motor whine. So when the setpoint is real low the PWM is never high enough to get to the set point. It just stops moving and whines because its still getting the PWM signal just not enough to move it.

AgentNoise:
There are 332 steps in the rotation but what if I want to move less than a full rotation. What I have been doing is as the steps near the setpoint the PWN(speed) begins to slow, until it reaches 0 at the desired setpoint. The problem is, the motor does not turn when the PWM is too low and you just get a lot of motor whine. So when the setpoint is real low the PWM is never high enough to get to the set point. It just stops moving and whines because its still getting the PWM signal just not enough to move it.

That sounds to me like your system is inappropriate for what you want.

I am building a model train control system with a QRE1113 optical sensor producing one pulse every revolution. Part of my code takes account of the fact that the interval between pulses is too long indicating that the motor has stalled. When that happens my code gradually increases the PWM duty cycle without any reference to PID until the interval between pulses is acceptable (meaning the motor is moving again) and then it hands control back to the PID system.

It seems to me you need to conduct an in-depth review of the logic of your system. I don't know if the encoder count is the appropriate input for the PID system, although, of course, it is relevant for positioning. At the very least you should probably be working on the number of steps still to go. But somehow you need to capture the requirement that the PWM duty cycle still needs to be quite high even with only one step left to move.

IMHO what you are trying to do is not simple.

With my model train I propose to use PID to control the motor speed and simply cut off power when the correct number of revolutions has been reached. That may result in a slight inaccuracy in the number of revolutions but I think I can live with that.

However moving a DC motor precisely for a small fraction of a revolution will be very complex. People use stepper motors for that sort of thing.

...R

If we are talking position control, then encoder count is definitely the input feedback value. Speed control is something else.

MarkT:
If we are talking position control, then encoder count is definitely the input feedback value. Speed control is something else.

I have the impression that the way the OP is thinking his PWM duty cycle dwindles to near zero when the number of steps remaining becomes a small number.

Even if he wants position control he must maintain a non-zero speed for the motor right up to the final step.

...R

I'll say again, position control is not speed control, you are not controlling speed, you are controlling drive (torque). The D term in the PID handles the momentum of the system. Typically drive can be close
to full reverse to bring the motor to a stop at the right place, the D term basically handling the momentum
of the system (more momentum requires more D-term)

Any attempt for fast position control without a D term (and thus keeping drive +ve until the destination
is reached) will show overshoot and thus oscillation.

You can do better with feed-forward that explicitly models the momentum, and reserve the PID to handle
the difference between feed-forward and reality.

MarkT:
I'll say again, position control is not speed control

There was no need for the "I'll say again ..." I don't disagree with you.

And I have no doubt that you agree with me that a motor cannot move if its power input is too low - which is what seems to be the OP's problem.

I reckon there is a huge mis-match between your knowledge of how to do this and what the OP is currently trying.

But we need to hear from the OP.

...R