Electronics > Projects, Designs, and Technical Stuff
Inverted Pendulum robot won't balance! (MPU-6050, STM32, PID etc)
(1/5) > >>
MattHollands:
Hi!

I'm trying to make an inverted pendulum robot - photo attached. Rather than being the usual two-wheeled robot, it is actually just a single motor rotating in the vertical axis while the pivoting arm rotates in the horizontal axis (just look at the picture ;)). I also have some video (filmed at 4x slow) showing it in action - it has definitely performed better in the past. https://www.youtube.com/watch?v=80mq2CQVi0U&feature=youtu.be

But I just can't get it to balance. Generally it is quite jittery and has a tendency to oscillate/slam itself into the floor. I've tried for hours and hours over multiple weeks to get the PID loop tuned, but I just can't get something that works. I'm really not sure if it is something to do with the mechanics or the code.

I am using an MPU-6050 gyro/acc chip hooked up to an STM32 which controls a MP6513 motor controller. The motors are controlled by PWM. The loop time is approximately 1ms.

Since this is a pretty popular project idea with a fairly common gyro/acc IC I was hoping someone could point me in the right direction. My complete code can be found at https://github.com/mhollands/SpinBalance/blob/master/STM32_FW/Src/main.c but I will summarize here.

Register settings
This is run once before the main loop

--- Code: ---MPU_SetReg(0x6b, 0x00); //Set power mode
  MPU_SetReg(0x1c, 0x10); // Set accelerometers to +/-4g
  MPU_SetReg(0x1b, 0x10); //Set gyros to +/-500deg/sec
--- End code ---

Reading accelerometer and gyro
Read the 6 acc/gyro values and convert to grams and deg/sec.

--- Code: ---// Read Acceleration values
  MPU_GetReg(0x3b, acc_regs, 6);
  acc_vals[0] = ((int16_t)(acc_regs[0] << 8) + acc_regs[1])/8192.0; //Convert force to grams
  acc_vals[1] = ((int16_t)(acc_regs[2] << 8) + acc_regs[3])/8192.0; //Convert force to grams
  acc_vals[2] = ((int16_t)(acc_regs[4] << 8) + acc_regs[5])/8192.0; //Convert force to grams

  // Read Gyro values and convert to
  MPU_GetReg(0x43, gyro_regs, 6);
  gyro_vals[0] = ((int16_t)(gyro_regs[0] << 8) + gyro_regs[1])/65.5 + 0.1620; //Convert to degs/s
  gyro_vals[1] = ((int16_t)(gyro_regs[2] << 8) + gyro_regs[3])/65.5; //Convert to degs/s
  gyro_vals[2] = ((int16_t)(gyro_regs[4] << 8) + gyro_regs[5])/65.5 - 1.1653; //Convert to degs/s

--- End code ---

Calculate angle
Final angle is a combination of the data from accelerometer and the gyro.

--- Code: ---// Calculate the accelerometer angle
  float accAngle = (atan2(acc_vals[0],acc_vals[1]) * 180 / PI);

  // Get time delta
  uint32_t t_now = HAL_GetTick();
  uint32_t t_delta = t_now - t;
  delay_time += (TARGET_PERIOD - t_delta);
  t = t_now;

  // Combine accelerometer and gyro angles
  angle = 0.98*(angle + gyro_vals[2]*t_delta/1000) + 0.02*accAngle;
--- End code ---

Calculate PID
Using values for Ki, Kp, Kd, integrate error etc.

--- Code: ---balance(angle+1, gyro_vals[2], t_delta);
--- End code ---

--- Code: ---i_error += angle * t_delta / 1000;
int16_t desired_speed = (int16_t)(i_error*ki + angle*kp + delta_angle*kd);
current_speed = desired_speed;
SET_PWM_SPEED(current_speed);
--- End code ---

Are there any rookie errors I should be looking out for?
Are there any simple tests I could do to root cause the issue?
Any debugging tips.
SiliconWizard:
I haven't looked at it in great details, but first thing I would question is your final estimation of the pendulum angle. Are you sure it's correct?
ebastler:
In your video, I don't actually see oscillations. Rather it looks like the device detects that the pendulum is tilting, and tries to move the support in the same direction to get it back underneath the center of gravity -- but doesn't do that fast enough, so the pendulum leans over further and further, while the support follows behind.

Do you have your parameters tuned correctly? And are you sure the motor can physically accelerate fast enough to keep up with the tipping pendulum? (The pendulum is rather short, and hence tips over with a pretty high angular velocity.)
ogden:

--- Quote from: ebastler on January 05, 2020, 05:19:37 pm ---The pendulum is rather short, and hence tips over with a pretty high angular velocity.

--- End quote ---
Exactly. First fall of pendulum shows that motor do not keep-up with fall. I would immediately try to lengthen 3x times or so (move mass to the very end obviously), see how it goes then.
mstevens:
What does your modelling and math say?
Navigation
Message Index
Next page
There was an error while thanking
Thanking...

Go to full version
Powered by SMFPacks Advanced Attachments Uploader Mod