Go Down

Topic: Stepper motor speed control with PID programming doubt (Read 3175 times) previous topic - next topic

lovethejobz

Nov 23, 2017, 08:59 am Last Edit: Nov 23, 2017, 09:59 am by lovethejobz
I am having a stepper motor and a rotary encoder. I have to position the stepper at 60 degrees each.
For the speed control I need to use a pid algorithm.
The input to the pid should be from encoder(steps per sec), Is it possible?
setpoint should be 1 step/sec.

The last part of the code is for laser after positioning.

I have a program code, but I dont think its working well.

Please help :smiley-roll-sweat:

This is the code:
Code: [Select]
#include <LiquidCrystal.h>
#include <Servo.h>


#define trig A3
#define dir A4
#define en A5

//#define pos1 240
//#define pos2 480
//#define pos3 720
//#define pos4 960
//#define pos5 1200

#define pos1 240
#define pos2 489
#define pos3 733
#define pos4 973
#define pos5 1202

#define servo 5
#define laser 4

#define lim_low A0
#define lim_up A1

#define low 27
#define up 117

unsigned long lastTime;
double Input, Output, Setpoint;
double ITerm = 0, lastInput = 1;
double kp = 0.0003, ki = 0.075 , kd = 0;
double Kopen = 20;
int SampleTime = 10; //.01 sec
double outMin = 0, outMax = 360;
bool inAuto = false;

#define MANUAL 0
#define AUTOMATIC 1

#define DIRECT 0
#define REVERSE 1
int controllerDirection = DIRECT;

long newposition;
long oldposition = 0;
unsigned long newtime;
unsigned long oldtime = 0;
long vel;

void updateEncoder();
void one_step(int );
void   laser_trig();
void hault(int , int );
LiquidCrystal lcd(12, 11, 10, 9, 8, 7);
Servo myservo;
int encoderPin1 = 2;
int encoderPin2 = 3;

volatile int lastEncoded = 0;
float encoderValue = 0;
int test = 1;
long lastencoderValue = 0;

int lastMSB = 0;
int lastLSB = 0;

void setup()
{



  myservo.attach(servo);
  Serial.begin (9600);
  lcd.begin(16, 2);

  pinMode(encoderPin1, INPUT);
  pinMode(encoderPin2, INPUT);

  digitalWrite(encoderPin1, HIGH); //turn pullup resistor on
  digitalWrite(encoderPin2, HIGH); //turn pullup resistor on

  //call updateEncoder() when any high/low changed seen
  //on interrupt 0 (pin 2), or interrupt 1 (pin 3)
  attachInterrupt(0, updateEncoder, CHANGE);
  attachInterrupt(1, updateEncoder, CHANGE);


  pinMode(servo, OUTPUT);
  pinMode(A3, OUTPUT);
  pinMode(A5, OUTPUT);
  pinMode(A4, OUTPUT);
  pinMode(A2, INPUT);
  pinMode(laser, OUTPUT);
  pinMode(lim_low, INPUT);
  pinMode(lim_up, INPUT);

  digitalWrite(laser, 0);
  digitalWrite(en, 0);
  digitalWrite(dir, 0);

  lcd.setCursor(0, 0);
  lcd.print("   POSITIONING     ");
  lcd.setCursor(0, 1);
  lcd.print("     SYSTEM     ");
  delay(3000);

  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Initialising.....   ");
  while (digitalRead(lim_up) == LOW)
    myservo.write(up);
  myservo.detach();

}

void loop()
{
  delay(4000);
  myservo.attach(servo);
  while (1)
  {
    while (digitalRead(lim_low) == LOW)
      myservo.write(low);
    myservo.detach();

    for (int j = 0; j < test; j++)
    {
      digitalWrite(dir, 1);
      while (digitalRead(A2) == HIGH)
      {
        Serial.println(encoderValue / 4);
        one_step(1);

      }
      delay(1000);
      lcd.clear();
      lcd.setCursor(0, 0);
      lcd.print("Angle= ");
      lcd.print("0.00 deg");

      lcd.setCursor(0, 1);
      lcd.print("Speed= ");
      lcd.print("0.00 RPM");
      encoderValue = 0;

      digitalWrite(dir, 1);
      laser_trig();
      hault(pos1, 1);
      laser_trig();
      hault(pos2, 1);
      laser_trig();
      hault(pos3, 1);
      laser_trig();
      hault(pos4, 1);
      laser_trig();
      hault(pos5, 1);
      laser_trig();
    }
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Please wait...");
    delay(2000);


    myservo.attach(servo);
    while (digitalRead(lim_up) == LOW)
      myservo.write(up);
    myservo.detach();
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Test Finished");
    while (1)
    {}

  }
  newposition = lastEncoded;
  newtime = millis();
  vel = (newposition - oldposition) * 1000 / (newtime - oldtime);
  Serial.print ("speed = ");
  Serial.println (vel);
  oldposition = newposition;
  oldtime = newtime;
}

void one_step(int del)
{
  digitalWrite(A3, 1);
  delay(del);
  digitalWrite(A3, 0);

}
//-----------------------------------------------------------------------------------------------
void Compute()
{
  if (!inAuto) return;
  unsigned long now = millis();
  int timeChange = (now - lastTime);
  double input = vel;
  double Setpoint = 1;
  if (timeChange >= SampleTime)
  {
    /*Compute all the working error variables*/
    double error = Setpoint - Input;
    ITerm += (ki * error);
    if (ITerm > outMax) ITerm = outMax;
    else if (ITerm < outMin) ITerm = outMin;
    double dInput = (Input - lastInput);

    /*Compute PID Output*/
    Output = Kopen * 0.201 * (kp * error + ITerm - kd * dInput);
    if (Output > outMax) Output = outMax;
    else if (Output < outMin) Output = outMin;

    /*Remember some variables for next time*/
    lastInput = Input;
    lastTime = now;
  }
  Compute();
  analogWrite(3, Output);
}


void SetTunings(double Kp, double Ki, double Kd)
{
  if (Kp < 0 || Ki < 0 || Kd < 0) return;

  double SampleTimeInSec = ((double)SampleTime) / 1000;
  kp = Kp;
  ki = Ki * SampleTimeInSec;
  kd = Kd / SampleTimeInSec;

  if (controllerDirection == REVERSE)
  {
    kp = (0 - kp);
    ki = (0 - ki);
    kd = (0 - kd);
  }
}

void SetSampleTime(int NewSampleTime)
{
  if (NewSampleTime > 0)
  {
    double ratio  = (double)NewSampleTime
                    / (double)SampleTime;
    ki *= ratio;
    kd /= ratio;
    SampleTime = (unsigned long)NewSampleTime;
  }
}

void SetOutputLimits(double Min, double Max)
{
  if (Min > Max) return;
  outMin = Min;
  outMax = Max;

  if (Output > outMax) Output = outMax;
  else if (Output < outMin) Output = outMin;

  if (ITerm > outMax) ITerm = outMax;
  else if (ITerm < outMin) ITerm = outMin;
}

void SetMode(int Mode)
{
  bool newAuto = (Mode == AUTOMATIC);
  if (newAuto == !inAuto)
  { /*we just went from manual to auto*/
    Initialize();
  }
  inAuto = newAuto;
}

void Initialize()
{
  lastInput = Input;
  ITerm = Output;
  if (ITerm > outMax) ITerm = outMax;
  else if (ITerm < outMin) ITerm = outMin;

}

void SetControllerDirection(int Direction)
{
  controllerDirection = Direction;
  
  analogWrite(3, Output);
}

//-----------------------------------------------------------------------------------------------
void hault(int pos, int del)
{
  while (encoderValue <= pos)
  {
    Serial.println(encoderValue / 4);
    lcd.setCursor(0, 0);
    lcd.print("Angle= ");
    lcd.print(encoderValue / 4);
    lcd.print(" deg");
    lcd.setCursor(0, 1);
    lcd.print("Speed= ");
    lcd.print("1.87");
    one_step(del);
  }
}

//-----------------------------------------------------------------------------------------------
void updateEncoder() {
  int MSB = digitalRead(encoderPin1); //MSB = most significant bit
  int LSB = digitalRead(encoderPin2); //LSB = least significant bit

  int encoded = (MSB << 1) | LSB; //converting the 2 pin value to single number
  int sum  = (lastEncoded << 2) | encoded; //adding it to the previous encoded value

  if (sum == 0b1101 || sum == 0b0100 || sum == 0b0010 || sum == 0b1011) encoderValue ++;
  if (sum == 0b1110 || sum == 0b0111 || sum == 0b0001 || sum == 0b1000) encoderValue --;

  lastEncoded = encoded; //store this value for next time
}
//----------------------------------------------------------------------------------------------
void laser_trig()
{
  delay(2000);
  digitalWrite(laser, 1);
  delay(3000);
  digitalWrite(laser, 0);
  delay(1000);
  digitalWrite(laser, 1);
  delay(3000);
  digitalWrite(laser, 0);
  delay(1000);
  digitalWrite(laser, 1);
  delay(3000);
  digitalWrite(laser, 0);
  delay(1000);
}




Robin2

You need to expand on your description of the requirement.

If the motor is to stop at 60 degrees (which a 200 step motor cannot do exactly) where does speed come into the picture.

What is causing the encoder to turn at speed? And what is the range of encoder speeds?



You may be interested in my EasyPID Thread,

...R
Two or three hours spent thinking and reading documentation solves most programming problems.

lovethejobz

The stepper motor (200steps/rev) is rotated in microsteps(1/32) through microstepping board.
The encoder is a 360PPR/1440 steps rotary encoder(Performance: 360 pulses / rev. Operating voltage: DC5-24V, maximum mechanical speed 5000 rev / min, the electrical response frequency 20K / sec, the integrated speed 2000 rev / min.)

as per the code a pulse is given in 1 ms to the stepper and it rotates so slow. positions are given separately for each stop position. since the stepper rotates so slow, torque is less and it tends to miss steps. i want the PID algorithm to compensate for those missed step if the stepper looses a step.Can this code be programed like that?

Thanks in advance

Robin2

That's helpful. But I still can't visualize the system you are trying to create. How about telling us what it is?

It would take far too long to try to figure out what you want to achieve by reading your code. If you provide a comprehensive description it makes the code much easier to study.


5000 rpm is 83.3 rps and with 360 pulses per rev that would be 30,000 pps or one every 33 microsecs if my maths is correct. That's going to be hard work for a 16MHz Arduino.

...R
Two or three hours spent thinking and reading documentation solves most programming problems.

lovethejobz

Actually this is a positioning system for a test and samples are kept in a circular holder which is attached to a stepper. Stepper is coupled to rotary encoder through a gear.

Stepper moves 6400steps per revolution 1 step per ms. 1.8 degree is the step angle..
Encoder is having 1440 steps..
So the encoder cant sense each step rather senses 5 micro steps of motor as 1 step.
I want the code to read that and compensate for step losses to maitain the speed.
Speed will be like 1.87rpm only

Thanks

Robin2

I appreciate that you are making an effort but you are still muddling things up.

If there are 6400 steps per revolution you must be using 32x microstepping and the step angle will not be 1.8°

If you are only moving at 1.87 RPM where on earth did the 5000 come from?

If your encoder has 1440 steps per revolution it will move one step for every 4.444 motor steps (on average) - that is certainly not 5 and you need to make allowances for the difference.

It is hard to understand why you need an encoder when you have a stepper motor? The speed of a stepper motor can be established very precisely by the interval between steps.

Sorry, but I am still a long way from understanding your problem.

...R
Two or three hours spent thinking and reading documentation solves most programming problems.

outsider

If your process could tolerate a +/- 0.6 degree error this would be a lot simpler.

lovethejobz

#7
Nov 23, 2017, 02:16 pm Last Edit: Nov 23, 2017, 02:22 pm by lovethejobz
yea...the tolerance i kept was +/- .25 degree..but if it is +/- .6 degrees how do i change the code?

THanks

outsider

Start at position 1, 0 degrees:
move 33 steps, 59.4     "         pos 2:
move 34    "    , 120.6   "            "   3:
move 33    "    , 180      "            "   4:
move 33    "    , 239.4   "            "   5:
move 34    "    , 300.6   "           "   6:
move 33    "    , 0          "            "   back home.

You would need a way to "home" the motor at startup.
BTW what's the gear ratio?
 

lovethejobz

#9
Nov 23, 2017, 02:50 pm Last Edit: Nov 23, 2017, 02:56 pm by lovethejobz
Gear ratio is 1:1
for me positioning I have done.
I need to use a PID for speed (steps/s) incase if any slip occurs since the process involves a great level of precision.

outsider

Oh well, just thinking about gears, a 5:9 ratio between motor and "carousel" would give you a 1 degree per step of the motor.
Good luck.

Robin2

I need to use a PID for speed (steps/s) incase if any slip occurs since the process involves a great level of precision.
I could understand the need for acceleration (and deceleration) but you have not yet provided any indication that PID would be useful. And, because PID involves trial and error over several iterations it may actually make things worse.

Nor have you explained why you need an encoder in the first place.

You need to tell us what machine you are building or modifying and provide the specifications within which it is required to work. A photo or a diagram of the machine would also help.

...R
Two or three hours spent thinking and reading documentation solves most programming problems.

RayLivingston

I need to use a PID for speed (steps/s) incase if any slip occurs since the process involves a great level of precision.
And there is the flaw in your thinking.  A stepper motor, if properly sized, properly powered, and properly controlled, will NEVER "lose steps".  Losing steps is a sure-fire sign you either:
1) Chose a motor with inadequate torque for the job it's supposed to do, or
2) You are running the motor at inadequate voltage and/or current, or
3) You are not properly controlling the motor, and asking it to accelerate too quickly, decelerate too quickly, provide too much torque, or too much RPM (see #1 above).
Stepper motors are capable of running indefinitely, in open-loop mode, without EVER "losing steps", WHEN the motor is properly sized to the job, and it is properly controlled.  Since steppers ALWAYS run at maximum torque, once you "lose a step", there is nothing you can do to recover without seriously impacting performance and time to target.
So, bottom line, ditch the PID.   Make sure the motor and driver are up to the job.  Make sure you have the correct voltage, and are driving the motor with rated current.  Control acceleration and RPM to keep the motor within its specification limits.  Do the above, and you'll never have a problem with "lost steps".  Don't do the above, and no amount of Band-Aids, like PIDs, will make the system reliable.
Regards,
Ray L.

Go Up