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