I have built this rotary inverted pendulum with a stepper motor:
I'm having issues with the stepper motor going slower in reality than how it should in theory. For example, on the swing up:
This is for a school project, so it would be nice to not have to change the gains from the physical model to the simulated... I am taking damping into account.
Is there any way I can update the stepper faster? Right now I'm at the bare minimum pulse/rev that I can obtain with my driver (200). My driver is DM556T.
Here is my code:
float stepper_input = 0;
unsigned long systemStartTime = 0;
float controller_output = 0;
float tau_desired = 0;
float J_eff_est = .001;
// === Controller Targets and Limits ===
float velocityLimit = 12; // Max output velocity (rad/sec)
float targetPhi = 0;
float targetTheta = 0;
// PID controller params
float Kp_arm = 0.04;
float Ki_arm = 0.03;
float Kd_arm = 0.025;
float Kp_pendulum = .7;
float Ki_pendulum = 0.4;
float Kd_pendulum = 0.015;
float integral_arm = 0;
float prev_error_arm = 0;
float integral_pendulum = 0;
float prev_error_pendulum = 0;
// Swing params
float swing_gain = .065;
float phi_centering_gain = .01;
float has_swung = 0;
float swing_up_threshold = 0.99;
// === Measured States ===
float theta = 0;
float thetadot = 0;
float phi = 0;
float phidot = 0;
// System params
float L_p = 0.146;
float pend_density = 8000;
float pend_diam = 0.00635;
float m_p = PI * 0.25 * pend_diam * pend_diam * L_p * pend_density;
float I_p = (1/3) * m_p * L_p * L_p;
float L_a = 0.20;
float arm_density = 7850;
float arm_diam = .006;
float m_a = PI * 0.25 *arm_diam * arm_diam * L_a * arm_density;
float I_a = (1/3) * m_a * L_a * L_a;
float g = 9.81;
// === Pin Definitions ===
#define PUL_PIN 6
#define DIR_PIN 7
#define ENCODER_PIN_A 2
#define ENCODER_PIN_B 3
// === Stepper Config ===
const int stepsPerRev = 200;
const float gearRatio = 5;
const float twoPi = 6.28318530718;
// === Encoder Config ===
const int encoderPulsesPerRev = 1200;
// === Limits ===
const float maxOutputAngle = PI;
// === Global State ===
volatile long encoderTicks = 0;
long stepperTicks = 0;
float stepDelayMicros = 0;
bool stepDir = true;
// === Timing ===
unsigned long lastUpdateTime = 0;
unsigned long lastControlTime = 0;
const unsigned long controlInterval = 20000; // microseconds
// === ISR ===
void updateEncoder() {
bool A = digitalRead(ENCODER_PIN_A);
bool B = digitalRead(ENCODER_PIN_B);
encoderTicks += (A == B) ? 1 : -1;
}
float getOutputStepperAngle() {
float motorAngle = -(stepperTicks * twoPi) / stepsPerRev;
return motorAngle / gearRatio;
}
void setStepperVelocity(float velRadPerSec) {
// REVERSED DIRECTION
stepDir = (velRadPerSec < 0);
digitalWrite(DIR_PIN, stepDir ? HIGH : LOW);
float revsPerSec_output = abs(velRadPerSec) / twoPi;
float revsPerSec_motor = revsPerSec_output * gearRatio;
float stepsPerSec = revsPerSec_motor * stepsPerRev;
stepDelayMicros = (stepsPerSec > 0) ? 1e6 / (stepsPerSec * 2.0) : 0;
}
void updateStepper() {
static bool stepState = false;
static unsigned long lastStepTime = 0;
if (stepDelayMicros == 0) return;
unsigned long now = micros();
if (now - lastStepTime >= stepDelayMicros) {
lastStepTime = now;
digitalWrite(PUL_PIN, stepState ? LOW : HIGH);
stepState = !stepState;
if (!stepState) {
stepperTicks += (stepDir ? 1 : -1);
}
}
}
int sign(float x) {
return (x > 0) - (x < 0);
}
float wrapToPi(float angle) {
while (angle > PI) angle -= twoPi;
while (angle < -PI) angle += twoPi;
return angle;
}
void setup() {
Serial.begin(115200);
pinMode(PUL_PIN, OUTPUT);
pinMode(DIR_PIN, OUTPUT);
pinMode(ENCODER_PIN_A, INPUT_PULLUP);
pinMode(ENCODER_PIN_B, INPUT_PULLUP);
attachInterrupt(digitalPinToInterrupt(ENCODER_PIN_A), updateEncoder, CHANGE);
stepperTicks = 0;
setStepperVelocity(0);
lastUpdateTime = micros();
lastControlTime = micros();
delay(5000);
systemStartTime = millis();
}
void loop() {
updateStepper(); // Always run fast!
unsigned long now = micros();
if (now - lastControlTime >= controlInterval) {
lastControlTime = now;
phi = getOutputStepperAngle();
theta = (encoderTicks * twoPi) / encoderPulsesPerRev + PI;
static float lastTheta = 0;
static float lastPhi = 0;
static float lastPhidot = 0;
float dt = (now - lastUpdateTime) / 1e6;
thetadot = (theta - lastTheta) / dt;
phidot = (phi - lastPhi) / dt;
float phiError = phi - targetPhi;
float thetaError = sin(theta) - sin(targetTheta);
if (millis() - systemStartTime > 1000){
if (has_swung == 0) {
float PE_pend = -m_p * g * (L_p / 2) * cos(theta);
float KE_pend = 0.5 * I_p * thetadot * thetadot;
float E_total = PE_pend + KE_pend;
float E_desired = -m_p * g * L_p / 2; // upright
float energy_error = E_total - E_desired;
float u_swing = swing_gain * energy_error * sign(thetadot);
tau_desired = (u_swing - phi_centering_gain * phiError);
stepper_input = constrain(lastPhidot + (tau_desired / J_eff_est )* dt, -velocityLimit, velocityLimit);
setStepperVelocity(stepper_input);
if (cos(theta) > swing_up_threshold) has_swung = 1;
}
} if(has_swung ==1 && cos(theta) > .8) {
integral_arm += phiError * dt;
float derivative_arm = (phiError - prev_error_arm)/dt;
float u_arm = Kp_arm * phiError + Ki_arm * integral_arm + Kd_arm * derivative_arm;
prev_error_arm = phiError;
integral_pendulum += thetaError * dt;
float derivative_pendulum = (thetaError - prev_error_pendulum) / dt;
float u_pend = Kp_pendulum * thetaError + Ki_pendulum * integral_pendulum + Kd_pendulum * derivative_pendulum;
prev_error_pendulum = thetaError;
tau_desired = u_arm + u_pend;
stepper_input = constrain(lastPhidot + (tau_desired / J_eff_est )* dt, -velocityLimit, velocityLimit);
setStepperVelocity(stepper_input);
}
if (has_swung == 1 && cos(theta) < 0.8){
setStepperVelocity(0);
}
lastTheta = theta;
lastPhi = phi;
lastPhidot = phidot;
lastUpdateTime = now;
// // === Serial Output for Logging ===
// Serial.print("\t");
// Serial.println(dt, 5);
Serial.print(phi, 3);
Serial.print("\t");
Serial.println(theta, 3);
}
}
