PID control using Servo Library

Using Arduino Programming Questions
#1

Hello everyone,

I’m trying to control the RPMs of my 24 DC motor with a HB-25 motor controller and with an optical LM393 encoder to maintain constant speeds (1000rpm, 1500rpm, 2000 rpm, 2500 rpm and 3000 rpm) for a testing period of 1hr.

I choose to use the servo library instead of a PWM signal (analogWrite) to control the frequency of the pulses that are sent to my HB-25 motor controller as the pulses shouldn’t be refreshed more frequently than about 5.25 ms + pulse time.

I only want my motor to rotate in one direction (it doesn’t matter which one as I can change the polarity manually with my motor driver) so I mapped values of a potentiometer (+5v) to know which servo values (0-255) will allow me to go fully forward in one direction (pulse HIGH: 1.5 to 2 ms) and this are the values I found:

servo at:
94 neutral
136 max speed

I proceeded with the speed measurements with the LM393 optical optocoupler and a disk encoder with one hole.

Here is the code for my measurements:

#include <Servo.h>

volatile unsigned long elapsedtime=0;
volatile unsigned long time;
float tiempo; 
float rps;
float rpm;
Servo myservo;
int val; 

/* take time to make one turn from the speed sensor */ 
void docount(){  
  static unsigned long isrMilis=0;  //take the time taken to make one turn (ms)
  time= micros();   //save the new time of the interruption 
  elapsedtime=time-isrMilis;  //obtain the period of a rotation 
  isrMilis=time;    // save the time of the old interruption 
}

void setup() {
  Serial.begin(9600); 
 attachInterrupt(0, docount, FALLING);   // attach an interrupt called docount when the hole passes through the hole of the encoder disk 
  myservo.attach(9);     //servo pin 9 
}

void loop() {
  val = analogRead(1);            // reads the value of the potentiometer in A1 (value between 0 and 1023 )
  val = map(val, 0, 1023, 94, 136);     // scale it to use it with the servo (94 being neutral and 136 being maximal speed) 
  myservo.write(val);              //write the mapped value of the pot to the servo signal 
  noInterrupts();                 //deactivate interruptions
  tiempo=elapsedtime/1000;    //pass from us to ms 
  rpm=(1000/tiempo)*60;          //compute the rev per second and multiply them by 60 to obtain the rpms
  interrupts();      // activate interrupt
  Serial.print("Pulse: ");
  Serial.print(tiempo);
  Serial.println(" ms");
  Serial.print("Speed: ");
  Serial.print(rpm);
  Serial.println(" RPM");
  delay(1);
}

I still have variations of rpm so I decided to implement a PID control to assure that my speed remains constant.
I obtained the constants of my PID experimentally through the use of the transfer function of my motor and the autotuning block of Simulink.

Here is the modified code:

#include <Servo.h>
#include <PID_v1.h>

volatile unsigned long elapsedtime=0;
volatile unsigned long time;
float tiempo; 
float rps;
float rpm;
Servo myservo;
int val; 
double Input, Setpoint, Output;
int kp,ki,kd;
PID myPID(&Input, &Output, &Setpoint,1,1.41941244447319,0, DIRECT);

void docount(){  /* take time to make one turn from the speed sensor */ 
  static unsigned long isrMilis=0;  //take the time taken to make one turn (ms)
  time= micros();
  elapsedtime=time-isrMilis;
  isrMilis=time;
}

void setup() {
  Serial.begin(9600); 
 attachInterrupt(0, docount, FALLING);
  myservo.attach(9);
  Setpoint=1000;  //desired speed value to keep RPMs
  myPID.SetMode(AUTOMATIC); //turn PID on
}

void loop() {
  noInterrupts();
  tiempo=elapsedtime/1000;
  interrupts();
  rpm=(1000/tiempo)*60;
  Input = rpm;
  myPID.Compute();
  myservo.write(Output); 
  //Serial.print("Speed: ");
  //Serial.print(rpm);
  //Serial.println(" RPM");
   Serial.println(Output);
 delay(1);
}

but it never activates my motor.
when I change the servo function to an analogWrite my motor rotates, it never reaches the desired value and then shakes due to the specifications of the HB-25 of refreshment of pulses

With all this, several questions came to my mind:

Is there something wrong with my syntax to allow the usage of pid within the servo? or something i should add before writing the output to the servo?
Is there an easier way to maintain my speed in a constant value?

Is there a way to limit the output of my PID as I am also allowing reverse numbers and not only from 94 to 136?

Can I still use the analogwrite (considering the HB.25 criteria) rather than the servo to control the motor to rotate in one direction even though I should refresh my pulses in less than about 5.25 ms + pulse time?

oh here is the datasheet of my motor control https://www.parallax.com/sites/default/files/downloads/29144-HB-25-Motor-Controller-V1.2.pdf

and i forgot to mention that wondering also if there might be a problem with the definition of my variables as the PID output is a double and im not very familiar with this

Many many thanks for your time,

Andrea

#2 
void docount(){  /* take time to make one turn from the speed sensor */
  static unsigned long isrMilis=0;  //take the time taken to make one turn (ms)
  time= micros();
  elapsedtime=time-isrMilis;
  isrMilis=time;
}

I really don't understand using millis in the name when the variables holds a number of microseconds.

  Input = rpm;
  myPID.Compute();
  myservo.write(Output);

The input is not a servo position/speed value. The setpoint is not a servo position/speed value. Why do you assume a direct relationship between the Output value and a servo value?

Why, if you want very precise control over the servo speed are you pissing away precision using write()? writeMicroseconds() is a far better choice, once you figure out the relationship between Output and value to use in the writeMicroseconds() call.

#3

I would shorten the ISR to this

void docount(){  /* take time to make one turn from the speed sensor */
  time= micros();
  newIsrData = true;
}

and in loop() I would check for the value like this

void loop() {
   if (newIsrData == true) {
      previousIsrMicros = latestIsrMicros;
      noInterrupts();
         latestIsrMicros = time;
         newIsrData = false;
      interrupts();
      // code to calculate the elapsed time for the revolution
      // and call the PID
     }
     // other stuff
 }

and I would not bother calculating RPM - the PID calcs can work just as well on microsecsPerRevolution

...R

#4

This will throw away the bottom three digits of your count. Just like using milliseconds instead of microseconds.

  tiempo=elapsedtime/1000;

‘time’ doesn’t need to be volatile.

Try this:

#include <Servo.h>
#include <PID_v1.h>


volatile unsigned long MicrosecondsPerRevolution = 0;
const unsigned long MicrosecondsPerMinute = 60UL * 1000000UL;


Servo myservo;


double Input, Setpoint, Output;
const double kp = 1.0, ki = 1.41941244447319, kd = 0.0;


PID myPID(&Input, &Output, &Setpoint, kp, ki, kd, DIRECT);


void docount()
{ /* take time to make one turn from the speed sensor */
  static unsigned long isrMicros = 0; //take the time taken to make one turn (ms)
  unsigned long currentMicros = micros();
  unsigned long delta = currentMicros - isrMicros;
  if (delta > 1000)  // Skip signal bounces
  {
    MicrosecondsPerRevolution = currentMicros - isrMicros;
    isrMicros = currentMicros;
  }
}


void setup()
{
  Serial.begin(9600);
  attachInterrupt(0, docount, FALLING);
  myservo.attach(9);


  Setpoint = 1000.0; //desired speed value to keep RPMs


  myPID.SetOutputLimits(1000.0, 2000.0);  // range for servo.writeMicroseconds()
  myPID.SetMode(AUTOMATIC); //turn PID on
}


void loop()
{
  static unsigned long outputTimer = 0;


  noInterrupts();  // Grab volatile data with
  unsigned long microsecondsPerRevolution = MicrosecondsPerRevolution;
  interrupts();


  Input = MicrosecondsPerMinute / microsecondsPerRevolution;  //  RPM
  myPID.Compute();
  myservo.writeMicroseconds((int)Output);


  // Report status every five seconds
  if (outputTimer - millis() >= 5000)
  {
    outputTimer = millis();
    Serial.print("Setpoint: ");
    Serial.print(Setpoint);
    Serial.print(" Input:");
    Serial.print(Input);
    Serial.print("  Output: ");
    Serial.println(Output);
  }


  delay(1);
}
#5

Thanks John it works, i just needed to adjust the output range and the pid constants

#6