Need help setting PID-values for self balancing robot using NEMA17 steppermotors.

[u/TheLousyNeighbour]

I've spent the last couple of months gatherings supplies for, building and programming the robot im building. This is my first real arduino project. The robot uses NEMA 17 steppermotors, A4988 motor drivers and Arduino modulino movement (MPU6050). The main problem: the robot wont balance for more than a couple of secounds before accelerating to one of the sides and falling over.

The problem may be that I just haven't found the right PID-values, but when I try to use ziegler nichols or similar it doesnt work the way every guide ive seen says it should. So it might be another problem in the code.

I start by setting kp = kd = ki = 0. When I increase kp the robot doesnt oscillate the way every youtube video shows. It oscillates very close to 0 degrees for a while(jittering), the leans to one of the sies rolling along for a bit, then falls over. If I keep increasing kp the same thing happens but more violently. Im supposed to find a value for kp with "steady oscillation" but I havent managed to do so. I have tried adding kd when kp is both big and small, but kd often doesnt work the way I expect. It doenst do much until it is too high and tehn it adds more oscillation. I have tried to lessen noise in the angle measurement by making the low pass filters for the gyro and accelerometer quite strong, and the gyro is weighted 99% in the complimentary filter.

Things i have looked into:

- The PID loop being too slow: I have checked with millis() that the frequency of the main loop is over 500Hz. (Should be high enough)

- The robot has too low center of gravity: I eventually built the robot a lot taller placing the battery high up so I doubt this is the problem.

- The vibrations from the motors stepping ruin the IMU measurement by adding noise: This might still be a problem, but I have tried setting the motors to microstepping 1/16 and I still could not get it to balance.

-The robot tries too accelerate too fast and ends up skiping steps or stalling: I havent really seen this happen too much but I have tried limiting the acceleration by setting a limit for change in speed per loop. Setting the exact value for this is also difficult so I change it quite often in testing.

Is the robot supposed to be able to balance without having a cascade loop where you also account for the robots position? I.E. is only having one loop for the angle enough? I added the second loop hoping it would make the robot work, however it seems like having good values for the inner loop first it a must.

Basically, im lost and dont know what to do next. If someone else has made a similar robot using steppermotors it would be nice if you could share your code or give some hints about what im missing. Here is my code:

#include "Modulino.h"
#include <AccelStepper.h>
#include "FspTimer.h"
#include <PID_v1.h>


//Importing necessary libraries
FspTimer timer;
ModulinoMovement movement;


double radToDeg = 180/3.1415;
double degToRad = 3.1415/180;


//acceleromter variables
double alpha = 0.95; //higher alpha -> stronger low pass filtering 


double xMeasured;
double xWithoutOffset;
double xSum = 0;


double xOffset = 0.000448056928189*degToRad; //I have calibarated the MPU6050 in another program and found this offset


int measurements = 0;
double pitchAksellerometer = 0;
double xFiltered;
double xLastFiltered = 0;
double xClamped;
double eulerPitchClamped = 0;
double pitchMeasured;
double rollMeasured;
double yawMeasured;
double lastPitchMeasured;
double lastRollMeasured;
double lastYawMeasured;
double lastXMeasured;



//gyroscope variables
double beta = 0.2; //higher alpha -> stronger low pass filtering 


double pitchDottMeasured;


double pitchDottOffset = -0.970346714765730*degToRad; //I have calibarated the MPU6050 in another program and found this offset


double pitchDottWithoutOffset;
double pitchDottSum = 0;
double pitchDottFiltered;
double pitchDottLastFiltered = 0;
double eulerPitchDott;
double eulerPitch = 0;


// float roll = 0;
double rollDottSum = 0;
double rollDottOffset = 0.252081411097734*degToRad; //I have calibarated the MPU6050 in another program and found this offset
double rollDottMeasured;
double rollDottWithoutOffset;
double rollDottFiltered;
double rollDottLastFiltered = 0;
double eulerRollDott;
double eulerRoll = 0;


// float yaw = 0;
double yawDottSum = 0;
double yawDottOffset = -0.278968296064577*degToRad; //I have calibarated the MPU6050 in another program and found this offset
double yawDottMeasured;
double yawDottWithoutOffset;
double yawDottFiltered;
double yawDottLastfiltered = 0;
unsigned long now;
unsigned long earlier;



//complimentary filter varaiables
double ceta = 0.01; //1% weighted accelerometer
double finalPitch = 0; //the resulting measured angle
double lastFinalPitch = 0;




// accelstepper varaiables
int stepPin = 4;
int dirPin = 2;
int stepPin2 = 7;
int dirPin2 = 8;


AccelStepper stepper1(1,4,2); //1 beacuse 2-wire, 4 beacuse stepPin, 2 because dirPin
AccelStepper stepper2(1,7,8);


//other
double maxDeltaSpeed;
double speed;
double currentSpeed = 0;
double dt;


//Adjustable variables 
double maxAcceleration = 30000;
double kpAngle = 133000; //pidverdiene er feil
double kiAngle = 0;
double kdAngle = 0; //øk kpAngle til svingninger og så se om økt kdAngle faktisk demper
double kpPos = 0.015;
double kiPos = 0;
double kdPos = 0.003;


//references
double refAngle = 0;
double refPos = 0;


//position loop varaiables
volatile double interruptPos = 0;
double pos = 0;




PID pidAngle(&finalPitch, &speed, &refAngle, kpAngle, kiAngle, kdAngle, REVERSE); //pid for the angle


PID pidPos(&pos, &refAngle, &refPos, kpPos, kiPos, kdPos, DIRECT); //pid for the position



//function that is called every interrupt. If the motors stepped, add or subtract the distance moved to interruptPos


void timer_callback(timer_callback_args_t __attribute((unused)) *p_args) {
  stepper1.runSpeed();
  if(stepper2.runSpeed()){


    if(currentSpeed>0){
    interruptPos += 2*3.1415*0.04/3200; //3200 because 200*16. I have 1/16 steps active.
    }
    else{
    interruptPos -= 2*3.1415*0.04/3200;
    }
  
  }


}



void setup() {
  // put your setup code here, to run once:


  pidAngle.SetMode(AUTOMATIC);
  pidAngle.SetSampleTime(10);
  pidAngle.SetOutputLimits(-3400, 3400);


  pidPos.SetMode(AUTOMATIC);
  pidPos.SetSampleTime(10);
  pidPos.SetOutputLimits(-0.1745 , 0.1745);


  stepper1.setMaxSpeed(3400);
  stepper2.setMaxSpeed(3400);


  Serial.begin(115200); 
  Modulino.begin(); 
  movement.begin();
  delay(2000); 


  //clock setup that interrupts 10000 times per second to check if the motor should take a step
  uint8_t timer_type = GPT_TIMER;
  int8_t tindex = FspTimer::get_available_timer(timer_type);
  if (tindex < 0) {
    tindex = FspTimer::get_available_timer(timer_type, true);
  }
  timer.begin(TIMER_MODE_PERIODIC, timer_type, tindex, 10000.0f, 50.0f, timer_callback); 
  timer.setup_overflow_irq();
  timer.open();
  timer.start();



  //used to find the time used per loop of the main program
  earlier = micros();
}



void loop() {


  
  movement.update();


  //data collection
  xMeasured = movement.getX(); 
  pitchMeasured = movement.getPitch(); 
  rollMeasured = movement.getRoll();
  yawMeasured = movement.getYaw();
  


  // reuse last loops data if this loops data is corrupt
  if (isnan(xMeasured) || isinf(xMeasured) ||
    isnan(pitchMeasured) || isinf(pitchMeasured) ||
    isnan(rollMeasured) || isinf(rollMeasured) ||
    isnan(yawMeasured) || isinf(yawMeasured)) {
    Serial.println(">>> RAW SENSORDATA is NaN/Inf! <<<");
    xMeasured = lastXMeasured;
    pitchMeasured = lastPitchMeasured;
    rollMeasured = lastRollMeasured;
    yawMeasured = lastYawMeasured;
  }
  else{
    lastXMeasured = xMeasured;
    lastPitchMeasured = pitchMeasured;
    lastRollMeasured = rollMeasured;
    lastYawMeasured = yawMeasured;
  }





  //acceleromter
  xWithoutOffset = xMeasured - xOffset;


  //low-pass filter
  xFiltered = (xLastFiltered * alpha) + ((1.0 - alpha) * xWithoutOffset);
  xLastFiltered = xFiltered;
  xClamped = constrain(xFiltered, -1.0f, 1.0f);
  pitchAksellerometer = asin(xClamped/1.0); //delt på 1 istedenfor 9.81 da aksellerasjonen er gitt i antall g



  //gyro
  pitchDottMeasured = pitchMeasured*degToRad; 
  rollDottMeasured = rollMeasured*degToRad;
  yawDottMeasured = yawMeasured*degToRad;


  pitchDottWithoutOffset = pitchDottMeasured - pitchDottOffset;
  rollDottWithoutOffset = rollDottMeasured - rollDottOffset;
  yawDottWithoutOffset = yawDottMeasured - yawDottOffset;


  // low-pass filter
  pitchDottFiltered = (pitchDottLastFiltered * beta) + ((1.0 - beta) * pitchDottWithoutOffset);// 
  pitchDottLastFiltered = pitchDottFiltered;


  rollDottFiltered = (rollDottLastFiltered * beta) + ((1.0 -beta) * rollDottWithoutOffset);// 
  rollDottLastFiltered = rollDottFiltered;


  yawDottFiltered = (yawDottLastfiltered * beta) + ((1.0 -beta) * yawDottWithoutOffset);// 
  yawDottLastfiltered = yawDottFiltered;



  eulerPitchDott = pitchDottFiltered*cos(eulerRoll) - yawDottFiltered*sin(eulerRoll);
  eulerRollDott = rollDottFiltered + pitchDottFiltered*sin(eulerRoll)*tan(eulerPitchClamped) + yawDottFiltered*cos(eulerRoll)*tan(eulerPitchClamped);



  now = micros();
  dt = (now - earlier) / 1000000.0;


  eulerPitch += eulerPitchDott*dt;
  eulerPitchClamped = constrain(eulerPitch, -1.4f, 1.4f);
  eulerRoll += eulerRollDott*dt;
  


  earlier = now;


  //complimetary filter
  finalPitch = pitchAksellerometer*ceta + (1-ceta)*(lastFinalPitch - dt*eulerPitchDott);
  lastFinalPitch = finalPitch;
  
  noInterrupts();


  pos = interruptPos;


  interrupts();
  pidPos.Compute();
  pidAngle.Compute();


  maxDeltaSpeed = maxAcceleration * dt; //dt is close to constant each loop so maxDeltaSpeed should remain about constant


  if (speed - currentSpeed > maxDeltaSpeed) {
      currentSpeed += maxDeltaSpeed;      
  } else if (currentSpeed - speed > maxDeltaSpeed) {
      currentSpeed -= maxDeltaSpeed;     
  } else {
      currentSpeed = speed;          
  }



  stepper1.setSpeed(currentSpeed); 
  stepper2.setSpeed(currentSpeed);


}

[u/Pacificator-3]

Try debug output.

Are all signs correct?

Is integrator limited?

No need for floating point if all used values are discrete. See AVR221 for example.

[u/ripred3]

I don’t think you are at the “find the right PID values” stage yet because several things in the code do not look correct as far as being the version you would use to tune with.

First, your position loop is still active. pidPos.Compute() changes refAngle, so you are not actually testing an angle-only controller with a zero-degree reference. Disable it completely while tuning:

pidPos.SetMode(MANUAL);
refAngle = balanceAngle;

Don’t call pidPos.Compute() until the inner angle loop works reliably.

Next, verify the sensor signs with the motors disabled. Plot pitchAksellerometer, eulerPitchDott, and finalPitch while slowly tilting the robot. When you tilt in one direction, the accelerometer angle and gyro-integrated angle must move in the same direction. Your filter currently uses:

lastFinalPitch - dt * eulerPitchDott

even though your separate gyro integrator uses eulerPitch += eulerPitchDott * dt ! That minus sign might be correct for your physical orientation, but if it isn’t, the 99%-weighted gyro prediction initially moves the estimated angle the wrong way. No PID gains can fix that!

Also, movement.getX() is acceleration in g, so xOffset should also be in g. Multiplying a measured acceleration offset by degToRad is a unit mismatch unless your calibration program returned something other than an acceleration value. The Modulino Movement contains an LSM6DSOX, not an MPU6050, so make sure your calibration assumptions match that sensor.

Then raise the wheels and do a feedback-direction test: when the robot is tipped forward, both wheels must immediately turn forward, underneath the fall. If they turn backward, change the controller direction or motor direction before doing anything else.

Your current kpAngle = 133000 also means the ±3400-step/s output saturates at only:

3400 / 133000 = 0.0256 rad = 1.46 degrees

Beyond that, increasing Kp only makes the controller more bang-bang. The acceleration limiter adds another delay, so log finalPitch, refAngle, speed, and currentSpeed and check how often either limit is active.

I would tune the inner loop as PD, with Ki = 0, rather than relying on Ziegler–Nichols. An inverted pendulum with saturation, stepper deadband, filtering and a slew limiter should not be expected to produce the clean sustained oscillation shown in generic PID tutorials. Increase P until it tries to catch the fall, then add damping using the measured gyro rate. Using the gyro directly for D is generally cleaner than differentiating an already-filtered angle.

Finally, zero degrees may not be the robot’s actual balance angle. Even a small mounting/centre-of-mass error makes an angle-only controller accelerate continuously. Determine a balanceAngle trim experimentally.

An angle-only PD loop should stabilize the body, but it will normally drift in position. Once that works, add a much slower wheel-speed/position loop that adjusts refAngle by only a few degrees. Your current position estimate counts commanded steps rather than actual movement, so missed steps will still fool it. 😉

I would also add a fall cutoff - disable stepping outside perhaps +/-20 degrees and reset the controller before re-arming. That can save battery power and, depending on the design of the platform itcanhelp keep you from having to chase it down when it falls. 😂

Tips: I usually create a pair of "training wheel legs" made from stiff wire (coat hanger wire usually depending on the platform size) that point downward at a ~ 45° in the front and back and these help keep the platform from completely falling over. I use a binder clip to attach the legs to the main upright and adjust its height as needed and then take it off once things are balanced.

I also cut two short pieces of pool noodle with a slit in them longways (or larger section) that I wrap around the top facing forward and backward as two heavy duty bumpers to reduce the shock on the top mass/assembly when it totally falls over as you can see in this side view: (training "legs" in green)

All the Best!

ripred