Stepper Motor Control Problem

Hello,

I am trying to control a rotary inverted pendulum with a stepper motor (NEMA 17, 2A 1.8 deg, set to half steps (400 steps per revolution), a motor driver (DM542T) an encoder (2000 pulses per revolution) and an Arduino Mega 2560. I am following the project description I found here:

Inverted Pendulum Project (in German)

I can read the encoder without any problems, but with the stepper motor I am seeing a weird behavior: Within the loop, the motor sometimes gets stuck, so despite an existing error, I don't see any control action. When I transmit the actual angle or the desired output to the serial plotter, I also see that the transmitted curve pauses. Obviously, with this behavior, controlling the pendulum in the inverted position is impossible.

So my question is: What causes this behavior? It must be the motor, because when I disable the motor action, the actual angle is transmitted without any pauses.

Any help is greatly appreciated.

#include <PID_v2.h>
#include <Encoder.h>
#include "BasicStepperDriver.h"

// Motor steps per revolution. Most steppers are 200 steps or 1.8 degrees/step
#define MOTOR_STEPS 200

// All the wires needed for full functionality
#define ENBL 7
#define DIR 8
#define STEP 9


#define clearButton 11 // clear button for setting zero

// Since microstepping is set externally, make sure this matches the selected mode
// 1=full step, 2=half step etc.
#define MICROSTEPS 2
// 2-wire basic config, microstepping is hardwired on the driver
BasicStepperDriver stepper(MOTOR_STEPS, DIR, STEP);

// Definition der Encoderanschlüsse
#define rot_Encoder_A 20    // rotary Encoderpin A an Pin 20
#define rot_Encoder_B 21    // rotary Encoderpin B an Pin 21
Encoder rotEnc(rot_Encoder_A, rot_Encoder_B);   
 
 // angle Limits for Controller
   double limit_angle= 350; //limit at which it becomes impossible to catch the pendulum again if exeeded, 
   byte output_deadband= 0;  //Deadband Variables
     
  //Miscellaneous Variables
    double angle, y=0;
    double a = 0.80;  // Filterkonstante zur Dämpfung
   
//PID Setup
    double rot_kp= 0.02;
    double rot_ki= 0.003;//55.0;
    double rot_kd= 0.0;
    double Output= 0.0;
    double rot_Setpoint = 0.0;

    int controlActive = 1;
  
 //PID Controller initialisation
    PID rot_Controller(&angle,&Output,&rot_Setpoint,rot_kp,rot_ki,rot_kd,DIRECT);
   
void setup() {
  pinMode(clearButton, INPUT);
  pinMode(ENBL, OUTPUT);
  digitalWrite(ENBL,1); //stepper aus
  Serial.begin(115200);     
//Initialize Encoder counts
//full pendulum rotation = 8000 Encoder counts
  rotEnc.write(-4000); //set when pendulum is hanging straight down due to gravity
   
 //PID Settings
     rot_Controller.SetMode(AUTOMATIC);
     rot_Controller.SetOutputLimits(-100,100); 
     rot_Controller.SetSampleTime(5); //2
     rot_Controller.SetTunings(rot_kp,rot_ki,rot_kd);
 }

void loop() {
  angle = rotEnc.read();
    if (digitalRead(clearButton) == HIGH ) { // use the clear button to set zero manually
    rotEnc.write(-4000);
  }
  Serial.println(angle); // Ausgabe der Stellgöße auf den Monitor
  if (controlActive ==1 && angle < limit_angle && angle > (limit_angle*-1)){    //perform as long as pendulum is within the angular limits  
                                 //Execute angular PID controller
                                  rot_Controller.Compute();
                                  y = a * y + (1.0 - a) * Output;
                                  //Serial.println(y); // Ausgabe der Stellgöße auf den Monitor
                                  digitalWrite(ENBL,0); // Motor aktivieren
                                  if(y > output_deadband){
                                     stepper.begin(y, MICROSTEPS);
                                     stepper.move(-MICROSTEPS);
                                   }
                                  else if(y < -output_deadband){
                                     stepper.begin(abs(y), MICROSTEPS);
                                     stepper.move(MICROSTEPS);
                                   }
                         } //stop motor if pendulum has tipped out of the angular limits
    else {
      digitalWrite(ENBL,1);
   }  
   delay(5);                 
 } //End Loop

Write a program to test the motor, and to make sure that you understand how it is controlled and that it is functioning properly.

Verify that the power supply and motor driver can deliver the required current, and that the motor delivers the required torque, over the range of anticipated distances and step rates.

In a separate step, put in print statements so that you can determine what commands the program was issuing, at the time the motor becomes "stuck".

My very very wild guess is that it is stuck in the dead band.

...R

BasicStepperDriver is blocking when you call move() - you want non-blocking driver code if
you have a control loop to keep up with.

Have you tried AccelStepper? Its got a non-blocking move() method. Actually the BasicStepperDriver
library seems to have a non-blocking example - I think you have to avoid move and use startMove
and nextAction only.

Thank you all for your replies! ??? MarkT is right, the main problem was that the used library for controlling the stepper was blocking. So I switched to the mentioned "AccelStepper" lib, and now I can nicely move the stepper.

However, I still fail at balancing the pendulum. No matter how I choose the PI control gains, the best I can achieve is balancing for a few seconds with increasing oscillations on the arm until the pendulum tips over. I have the impression that the stepper is not moving fast enough despite high control gains, but increasing the set speed and acceleration also hasn't worked out. :slightly_frowning_face:

Also I am wondering if I'm using the right control approach: My control variable u corresponds to a new setpoint for the steps to move, and this value is altered in every loop. Wouldn't it be more intuitive to control the motor speed? ::slight_smile:

#include <AccelStepper.h>

#define encoderPinA  2  // for interrupt A
#define encoderPinB  3  // for interrupt B
#define clearButton 11 // clear button for setting zero

// ----------------------- ENCODER ----------------------------------------------------
double incr2deg = 0.045;  // one rotation 360° = 8000 increments
double incr2steps = 0.2;  // one rotation 360° = 4000 increments = 800 steps
volatile double encoderDeg = 0;
volatile double encoderPos = 0;     
int stepsDes = 0;

// Define a stepper and the pins it will use
AccelStepper stepper(1,9,8); // Defaults to AccelStepper::FULL4WIRE (4 pins) on 2, 3, 4, 5
AccelStepper setEnablePin(7);

int motionDir = 1;
int deltaSteps = 1;

// ------------------- PID CONTROLLER -----------------------------------------------------
double e = 0;
double eInt = 0;
double eDiff = 0;
double eLast = 0;
double agDes = 180;
double timeNow = 0;
double timeLast = 0;
double dt = 0;
double u = 0;
double u_last = 0;
double uMax = 2000;
double uMin = -2000;
double minDeg = 175;
double maxDeg = 185;
double Kp = 1.3; 
double Ki = 0.00000002; 
double Kd = 0.0;
int controlActive = 1;

void setup()
{  
  // ---------------------- ENCODER -----------------------------------
  pinMode(encoderPinA, INPUT);
  pinMode(encoderPinB, INPUT);
  pinMode(clearButton, INPUT);
  digitalWrite(encoderPinA, HIGH); // turn on Pull-up resistor
  digitalWrite(encoderPinB, HIGH); // turn on Pull-up resistor
  digitalWrite(clearButton, HIGH); // turn on Pull-up resistor
    // encoder pin on interrupt 0 (pin 2)
  attachInterrupt(0, doEncoderA, CHANGE);

  // encoder pin on interrupt 1 (pin 3)
  attachInterrupt(1, doEncoderB, CHANGE);

  // -------------- SERIAL COMMUNICATION -------------------------
  Serial.begin (9600);

  //--------------- STEPPER MOTOR --------------------------------
  stepper.setMaxSpeed(300); // Steps per second.
  stepper.setAcceleration(10000); // steps per second per second

}

void loop()
{
     encoderDeg = encoderPos*incr2deg; // convert encoder Pos (increments) to encoder deg (Degree)
  //Serial.println (encoderPos, DEC);
  //Serial.println (encoderDeg, 3);

  if (digitalRead(clearButton) == HIGH ) { // use the clear button to set zero manually
    encoderPos = 0;
  }

  // Compute the control error
  e = agDes - encoderDeg;
  //Serial.println (e, DEC);

  // Compute the control input
  timeNow = millis()*1000;
  dt = timeNow - timeLast;
  timeLast = timeNow;
  eInt = eInt + dt*e;
  eDiff = (e-eLast)/dt;
  eLast = e;

 // Only perform control action if angle is within controllable area
  if (encoderDeg<maxDeg && encoderDeg>minDeg && controlActive) {
   u = Kp*e + Ki*eInt;
    if (u>uMax) {
      u = uMax;
    }
    if (u<uMin) {
      u = uMin;
    }
  }
  else {
    u = 0.0;
    eInt = 0;
  }
 // Serial.println (u, DEC);

	stepper.move(-u);
  stepper.run();
   u_last = u;
}

//-----------------------------------------------------------------------------------
void doEncoderA() {
  // look for a low-to-high on channel A
  if (digitalRead(encoderPinA) == HIGH) {
    // check channel B to see which way encoder is turning
    if (digitalRead(encoderPinB) == LOW) {
      encoderPos = encoderPos + 1;         // CW
      }
    else {
      encoderPos = encoderPos - 1;         // CCW
    }
  }

  else   // must be a high-to-low edge on channel A
  {
    // check channel B to see which way encoder is turning
    if (digitalRead(encoderPinB) == HIGH) {
      encoderPos = encoderPos + 1;          // CW
    }
    else {
      encoderPos = encoderPos - 1;          // CCW
    }
  }
}

//------------------------------------------------------------------------------------------------
void doEncoderB() {
  // look for a low-to-high on channel B
  if (digitalRead(encoderPinB) == HIGH) {
    // check channel A to see which way encoder is turning
    if (digitalRead(encoderPinA) == HIGH) {
      encoderPos = encoderPos + 1;         // CW
    }
    else {
      encoderPos = encoderPos - 1;         // CCW
    }
  }
  // Look for a high-to-low on channel B
  else {
    // check channel B to see which way encoder is turning
    if (digitalRead(encoderPinA) == LOW) {
      encoderPos = encoderPos + 1;          // CW
    }
    else {
      encoderPos = encoderPos - 1;          // CCW
    }
  }
}

// -------------------------------------------------------------------------------------------------
void zeroPos() {
  //encoderPos = 0;
}

You motor and drivegear might indeed be too slow, or you might not be using enough D term
to stabilize the system for high gain.

It is a tricky problem because pendulum control is non-linear, so the ideal PID settings for small
deviations are not ideal for large oscillations. More sophisticated control strategies will perform
better, but non-linear control theory isn't so easy to grok.

This topic was automatically closed 120 days after the last reply. New replies are no longer allowed.