Need help with balancing a stick (PID) project

I have a stick balancing project using a Stepper Motor, with timing belt, an MPU6050 and an A4988 stepper motor carrier. The idea is to see if I can balance the stick using PID. I have attached a picture of the project. The MPU6050 is at the top the stick which is 36" long. The arduino UNO reads the MPU and uses the stepper motor to slide the carriage at the bottom of the stick to try to balance it.

I have made a lot of progress. The physical hardware works well with the carriage on linear bearings. The stepper motor can be made to speed up, slow down and reverse with plenty of speed using acceleration logic. The PID controller works well enough to balance the stick and it us using 3 pots to adjust the Kp, Ki & Kd parameters in real-time.

There is however, a serious flaw in the system. While the PID controller works well enough to balance the stick, it has a drift problem. It balances for a second but then starts to drift to one side or the other until it hits a stop. The stepper is more than fast enough to balance the stick but I am failing to do something to stop it. I have not yet found a way to stop it from drifting.

I have included the main code here but the control and MPU code are both large. The problem I think is in the main calculate routine.

Thought on corrections to the sketch or ways to control drift will be appreciated.

#include <Wire.h>

// A4988 stepper carrier pin values
#define MS1 2                     // PORTD 2   Arduino digital pin 2
#define MS2 3                     // PORTD 3   Arduino digital pin 3
#define MS3 4                     // PORTD 4   Arduino digital pin 4
#define ENABLE 5                  // PORTD 5   Arduino digital pin 5
#define STEP 6                    // PORTD 6   Arduino digital pin 6
#define DIRECTION 7               // PORTD 7   Arduino digital pin 7

#define PPOT A1                   // Pot for adjusting P
#define IPOT A2                   // Pot for adjusting I
#define DPOT A3                   // Pot for adjusting D
#define ZERO_SPEED 65535          // indicates the motor is stopped
#define MAX_CONTROL_OUTPUT 160    // maximum requested motor speed control
#define MAX_ACCEL 20              // maximum acceleratiom of the motor speed
#define SAMPLE_TIME  1000000;     // 1 sec

// PID variables
float setPnt = -1.0;          // Desired position(system input) -1 is the stick balance point
float posVar;                 // Actual Position (system sensor)
float error, errSum;          // Error = SP – PV
float dErr, lastErr;          // used in Kd error calculations
float controlOutput;          // Control Output = Sum of P I D controllers
float kp;                     // Proportional gain = Increase until oscillations occur
float ki;                     // Integral Gain = increase to stop oscillations
float kd;                     // Derivative gain = Increase to improve responsiveness
float desiredMotorSpeed;      // the desired motor speed
unsigned long now, lastTime;  // used to time the PID calculation cycles
unsigned long sampleTime;     // determines the cycle time and adjusts Ki & Kd
volatile int8_t  motorDir;    // direction of stepper motor +1, -1, 0  0 = stopped
volatile int32_t steps;       // steps the motor has taken in either direction
int32_t timeChange;           // the amount of time that has passed since the last cycle
float ratio;                  // used in setting the cycle time
float gain = 0.95;            // use in determining filter gain (not yet incorporated)
float lastFiltered = 0;       // where the filter stores its last filtered output

// AUX definitions
#define CLR(x,y) (x&=(~(1<<y)))  // used to clear the A4988 Step pin
#define SET(x,y) (x|=(1<<y))     // used to set the A4988 Step pin
#define RAD2GRAD 57.2957795      // used in angle/acceleration calculations
#define GRAD2RAD 0.01745329251994329576923690768489 // not currently used, but pretty nice to have

void setup()
  Wire.begin();                     // start the I2C bus
  Serial.begin(115200);             // start the Serial communications
  // set up the A4988 carrier pins
  pinMode(MS1, OUTPUT);             // Arduino digital pin 2
  pinMode(MS2, OUTPUT);             // Arduino digital pin 3
  pinMode(MS3, OUTPUT);             // Arduino digital pin 4
  pinMode(STEP, OUTPUT);            // Arduino digital pin 6
  pinMode(DIRECTION, OUTPUT);       // Arduino digital pin 7
  pinMode(ENABLE, OUTPUT);          // Arduino digital pin 5 (Active LOW)

  // set the A4988 pin initial states
  digitalWrite(MS1, HIGH);          // MS1,2,3 high = 16 microsteps
  digitalWrite(MS2, HIGH);
  digitalWrite(MS3, HIGH);
  digitalWrite(STEP, LOW);
  digitalWrite(DIRECTION, HIGH);
  digitalWrite(ENABLE, LOW);        // Enable the motor

  // set the pot pins to INPUT
  pinMode(PPOT, INPUT);             // Pot for adjusting P
  pinMode(IPOT, INPUT);             // Pot for adjusting I
  pinMode(DPOT, INPUT);             // Pot for adjusting D

  // Setup Timer1 for the stepper motor pulses
  TIMSK1 |= (0 << OCIE1A);                            // disable timer interrup
  TCCR1A = 0;                                         // timer1 OCxA,B disconnected (arduino pins 9 & 10 are normal)
  TCCR1B = (1 << WGM12) | (0 << CS10) | (1 << CS11);  // CTC mode 4, rescaler = 8
  OCR1A = ZERO_SPEED;                                 // Indicate the motor is stopped
  TCNT1 = 0;                                          // set the count to 0

  MPU6050_setup();                                    // initialize the MPU6050
  MPU6050_calibrate();                                // calibrate the MPU6050
  digitalWrite(4, LOW);                               // enable stepper drivers
  motorDir = 0;                                       // the motor is stopped
  TIMSK1 |= (1 << OCIE1A);                            // Enable timer interrup
  SetSampleTime(10000);                               // 10ms
  lastTime = micros();                                // start the loop timer

void loop()

void Compute()
  now = micros();
  timeChange = (now - lastTime);
  if (timeChange > sampleTime)                                // 10ms sample time
    // New IMU data?
    if (MPU6050_newData())
      setPIDS();                                              // adjust Kp, Ki & Kd to pot settings
      MPU6050_read_3axis();                                   // Get the MPU6050 data
      posVar = MPU6050_getAngle((float)timeChange * .000001); // change in seconds
      error = setPnt - posVar * 2;                            // calculate the Kp error component
      errSum += error;                                        // and the Ki component
      dErr = error - lastErr;                                 // and the Kd component
      controlOutput = kp * error + ki * errSum + kd * dErr;   // calculate controlOutput
      // limit the controlOutput to the maximum the motor can handle
      controlOutput = constrain(controlOutput, -MAX_CONTROL_OUTPUT, MAX_CONTROL_OUTPUT);
      setMotorSpeed(controlOutput);                           // set the motor speed
      lastErr = error;
      lastTime = now;
      // setPnt += (steps * 0.001);   // failed attempt to stop the drift

void SetSampleTime(float NewSampleTime)
  // adjust the sampleTime
  if (NewSampleTime > 0)           //make certain this is not negative or zero
    ratio = NewSampleTime / SAMPLE_TIME;        // determine the new ratio
    ki *= ratio;                                // adjust Ki
    kd /= ratio;                                // and Kd
    sampleTime = (unsigned long)NewSampleTime;  //need to remember this

void setPIDS()
  // get the PID pot values - pots read returns 0 - 1023
  kp = analogRead(PPOT) * .1;                   // Serial.println(Kp, DEC);
  ki = analogRead(IPOT) * .00001;               // Serial.println(Ki, DEC);
  kd = analogRead(DPOT) * .001;                 // Serial.println(Kd, DEC);

//float filter(float input)
//  float  filtered;
//  filtered = lastFiltered * gain + input * (1 - gain);
//  lastFiltered = filtered;
//  return filtered;

void SendData()                                 // sends data for troubleshooting
  //    Serial.print("a"); Serial.println(setPnt, DEC);
      Serial.print("b"); Serial.println(posVar, DEC);
  //    Serial.print("c"); Serial.println(error, DEC);
  //    Serial.print("d"); Serial.println(controlOutput, DEC);
  //    Serial.print("e"); Serial.println(kp, DEC);
  //    Serial.print("f"); Serial.println(ki, DEC);
  //    Serial.print("g"); Serial.println(kd, DEC);
  //    Serial.print("h"); Serial.println(steps, DEC);

The problem I think is in the main calculate routine.

No. The problem is in the accelerometer/gyroscope hardware. You get what you pay for. You didn't pay much for that hardware, did you?

#define SAMPLE_TIME  1000000;     // 1 sec

Feels kind of high, to me.

You shouldn't be diddling with the timer without disabling interrupts while you diddle.

  SetSampleTime(10000);                               // 10ms

Why do you not use the #defined value?

      posVar = MPU6050_getAngle((float)timeChange * .000001); // change in seconds

That's not the inverse of your sample time...

I think this will be of better help then the mmu: Extremely Sensitive Cheap Homemade Seismometer - Arduino Project Hub in determining the position of the stick.

The detection of deviation of the stick position is the part of the linked project that might be on interest to you.

Excellent. I will incorporate these suggestions as best I can. Thanks.


The define is now:
#define SAMPLE_TIME 15000 // Microsec * 15000 = 15 ms
I changed the sample time from 10ms to 15ms.

The parameters are now set with:
SetcycleTime(SAMPLE_TIME); // 15ms

I disable all interrupts while I diddle with the clock stuff.

cli();                                              // disable interrupts
  TIMSK1 |= (0 << OCIE1A);                            // disable timer interrup
  TCCR1A = 0;                                         // timer1 OCxA,B disconnected (arduino pins 9 & 10 are normal)
  TCCR1B = (1 << WGM12) | (0 << CS10) | (1 << CS11);  // CTC mode 4, rescaler = 8
  OCR1A = ZERO_SPEED;                                 // Indicate the motor is stopped
  TCNT1 = 0;                                          // set the count to 0
  sei();                                              // enable interrupts

  MPU6050_setup();                                    // initialize the MPU6050
  MPU6050_calibrate();                                // calibrate the MPU6050
  digitalWrite(4, LOW);                               // enable stepper driver
  motorDir = 0;                                       // the motor is stopped
  TIMSK1 |= (1 << OCIE1A);                            // Enable timer interrup

Regarding the line:
posVar = MPU6050_getAngle((float)timeChange * .000001); // change in seconds

The MPU6050_getAngle routine I found takes the elapsed time in seconds so I just convert the timeChange to seconds.

I also ordered this:

It is supposed to be better than the MPU6050 and it includes a magnetometer. Let me know if you think there might be a better device.


I haven’t yet understood how the movement detection on the seismometer can be adapted to the stick balancer.

I think you are nearly there. The balancing is the hard part.

The drift position should be an input to your calculation. If you have moved away from center then you no longer want the stick to be perfectly vertical. You want to tilt it back to the center so that the balancing part will drift back to center.

The next step after that is to make that “center” controllable. Maybe a knob on an analog input. Then you can play with moving the stick under your control.

MorganS I believe that is the way.

I have been doing it the wrong way I think. I have a variable called steps. Whenever the stepper steps left or right, it adds or subtracts 1 respectively. I was trying to factor that into the PosVar which is the actual angle of the stick and I had no success. Tomorrow I will try to add the steps * n to the setPnt which is the target angle. It is currently set to the exact balance point of the stick. by adding (or subtracting) a portion of the steps to it, I should be able to tilt the stick back towards the center when it moves away from center. In addition, I should be able to add or subtract another component to the steps to change the center as you said.

Incidentally, I hooked up the new IMU today and it is MUCH more stable than the MPU6050. I need to finish up changing the code to accommodate that new device.

The relation of stick angle to step position is a PID loop like the inner loop doing the balancing. Start out with simple proportional control: have a "gain" multiplier that you can tune to make it work better.

If you're 100 steps from the target then you probably need less than 100 degrees of stick angle. Certainly less than 100 radians angle.