Full Version: Can't tune PID
You're currently viewing a stripped down version of our content. View the full version with proper formatting.
Hi, I'm making a balancing robot. It is not an actual B-robot, but it's quite the same. I read code of B-robot, tried to implement everything by myself. I doublechecked my code for mistakes and differences, and found nothing wrong.

My problem is: robot is quite balanced, but it can't handle any significant pushes.

I tried to tune my PD parameters using algorithm:
1. Increase KP until it starts oscillating
2. Tune KD until it stops

I tried blindly guess the right parameters as well, and many other ways. I tried both positive and negative KD. But it seems like the robot is not aggressive enough to keep balancing after push.

I post a video, where KP is the highest possible. When I increase it, robot goes crazy and tuning KD doesn't help.

Youtube link

Maximum acceleration is limited in my code, the limit is the highest possible. If I increase it, motors stop accidently. I tested them holding the robot in the air. They can go really fast in quite a short time, when I tilt the robot in the air.

Gyroscope data is 100% right and accurate, i tested it a lot plotting live graphs from my code etc..

I tried to shift the center of mass higher and lower. It doesn't help.

My robot consists of:
1. HS4401 Steppers (1,7 amps; 12 volts; 4,8 kg/cm torque)
2. A4988 Drivers (hot like Hell, VREF is 1.15 volts; I measured: motors consume 0.85 amps while in fixed position; they are set to 1/4 step)
3. MPU-6050
4. Arduino UNO
5. 11.1 volts LI-PO batteries

Please, help me. What should I do to make it at least half as stable as B-robot?

#include <Arduino.h>
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"

#define DEBUG
#define sprint(X) Serial.println(X)

#define XGyroOffset 220
#define YGyroOffset 76
#define ZGyroOffset -85
#define ZAccelOffset 1788

#define MOTOR_1_ENABLE 6
#define MOTOR_1_STEP 5
#define MOTOR_2_ENABLE 8
#define MOTOR_2_STEP 9
#define LED_PIN 13

//max acceleration in steps per square seconds (8 * 4 because of microstepping)
#define MAX_ACCELERATION_1 4000

#define ZERO_SPEED 65535
#define TIMER OCR1A

#define RAD_TO_DEG 1145.915
//minimum angle error needed to motors start working
#define MIN_ANGLE_ERROR 25

int KP = 32;
int KD = 0; //*0.01
int KI = 0;
int targetAngle = 0;
int currentAngle;
char dir;
unsigned long previousTime;
int previousError;
int previousSpeed;

MPU6050 mpu;

bool dmpReady = false;  // set true if DMP init was successful
uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount;     // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer

Quaternion q;           // [w, x, y, z]         quaternion container

volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high

#ifdef DEBUG
void serialEvent() {
    KD = (int)Serial.parseInt();

void dmpDataReady() {
    mpuInterrupt = true;

int getAngle() {
    mpu.dmpGetQuaternion(&q, fifoBuffer);
    return (int)(asin(-2 * (q.x * q.z - q.w * q.y)) * RAD_TO_DEG); //measured in 0.1 deg (from 0 to 3600)

void gyroInit() {
    devStatus = mpu.dmpInitialize();
#ifdef DEBUG
    Serial.println(mpu.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
    if (devStatus == 0) {
        attachInterrupt(0, dmpDataReady, RISING);
        mpuIntStatus = mpu.getIntStatus();
        dmpReady = true;
        packetSize = mpu.dmpGetFIFOPacketSize();
#ifdef DEBUG
        Serial.println("DMP ready");
    } else {
#ifdef DEBUG
        Serial.print("DMP Initialization failed (code ");

void interruptInit() {
    TCCR1A = 0; //init timer1 CTC mode
    TCCR1B = 0;
    TCCR1B |= (1 << WGM12);
    TCCR1B |= (1 << CS11); //8 prescaler, 16mHz => 2mHz
    TIMSK1 |= (1 << OCIE1A);
#ifdef DEBUG
    Serial.println("Interrupts initialized");

    //doesn't generate pulse if not moving
    if (dir == 0) return;
    digitalWrite(MOTOR_1_STEP, !digitalRead(MOTOR_1_STEP));
    digitalWrite(MOTOR_2_STEP, !digitalRead(MOTOR_2_STEP));


void motorInit() {
    pinMode(MOTOR_1_ENABLE, OUTPUT);
    pinMode(MOTOR_1_STEP, OUTPUT);
    pinMode(MOTOR_2_ENABLE, OUTPUT);
    pinMode(MOTOR_2_STEP, OUTPUT);
    digitalWrite(MOTOR_1_ENABLE, HIGH);
    digitalWrite(MOTOR_2_ENABLE, HIGH);
#ifdef DEBUG
    Serial.println("Motor drivers initialized");

void motorEnable() {
    digitalWrite(MOTOR_1_ENABLE, LOW);
    digitalWrite(MOTOR_2_ENABLE, LOW);
#ifdef DEBUG
    Serial.println("Motor drivers enabled");

//returns steps per second for setSpeed()
void calculateStepsPerSecondPID(int* output, char* dir) {
    long currentTime = millis();
    long dt = (currentTime - previousTime);
    int error = targetAngle - currentAngle;
    int dError = (error - previousError) * 10 / dt; //dt is in milliseconds, converted to 0.01 deg/sec
    if (abs(error) < MIN_ANGLE_ERROR) {
        *dir = 0;
    else {
        if (error > 0) *dir = 1;
        else *dir = -1;
    *output = abs(KP * error + KD * dError);
    previousError = error;
    previousTime = currentTime;

//calculating interrupt timer delay
//dir = {0, +-1}

void setSpeed(int stepsPerSecond, char dir) {
    int currentSpeed;
    if (previousSpeed - stepsPerSecond < -MAX_ACCELERATION) currentSpeed = previousSpeed + MAX_ACCELERATION;
    else {
        if (stepsPerSecond - previousSpeed < -MAX_ACCELERATION) currentSpeed = previousSpeed - MAX_ACCELERATION;
        else currentSpeed = stepsPerSecond;
    if (currentSpeed < 0) {
        currentSpeed = 1;
        dir = 0;
    if (dir == 0) TIMER = ZERO_SPEED;
    if (dir > 0) {
        digitalWrite(MOTOR_1_DIRECTION, HIGH);
        digitalWrite(MOTOR_2_DIRECTION, HIGH);
    if (dir < 0) {
        digitalWrite(MOTOR_1_DIRECTION, LOW);
        digitalWrite(MOTOR_2_DIRECTION, LOW);
    TIMER = 2000000 / currentSpeed; //2 mHz timer
    previousSpeed = currentSpeed;
    if (TCNT1 > TIMER) TCNT1 = 0;

void setup() {
#ifdef DEBUG
    while (!Serial);
    pinMode(LED_PIN, OUTPUT);
    previousError = 0;
    previousSpeed = 0;
    previousTime = millis();

void loop() {
    if (!dmpReady) return;
    mpuInterrupt = false;
    mpuIntStatus = mpu.getIntStatus();
    fifoCount = mpu.getFIFOCount();
    // check for overflow
    if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
#ifdef DEBUG
        Serial.println("FIFO overflow!");
    } else if (mpuIntStatus & 0x02) {
        while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
        mpu.getFIFOBytes(fifoBuffer, packetSize);
        fifoCount -= packetSize;
        currentAngle = getAngle();
        int PIDoutput;
        calculateStepsPerSecondPID(&PIDoutput, &dir);
        setSpeed(PIDoutput, dir);
Добрый день, nkuznetsov.
Судя по нику, позволю себе общаться с Вами по-русски.
1. Чем Вас не устроил оригинальный код?
2. Почему выбрали 1/4 шага?
3. Что делает обработчик прерывания, кроме того, что инвертирует step-сигнал?
4. Вы не находите, что Ваш whilt-цикл
while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
может выполнятся много дольше цикла таймера?
Общее же впечатление, - "купила бабка порося"... Извините за прямоту.
Спасибо за Ваш ответ!

1. Оригинальный код меня более чем устраивает, но мне очень хочется подробно разобраться в его работе, для этого я пытаюсь написать свой код на его основе.
2. Я выбрал 1/4 шага, потому что это выглядит достаточно плавно, в то же время достаточно быстро, при этом не задействует очень много импульсов в секунду.
3. Обработчик прерывания инвертирует сигнал на каждом из моторов (по даташиту мотора это делает 1 шаг). Если же необходимая скорость нулевая (direction == 0), то обработчик не делает ничего.
Переписал обработчик в таком виде:
    //doesn't generate pulse if not moving
    if (dir == 0) return;
    //motors steps are pin 5 and 9
    SET(PORTD, 5); // STEP MOTOR 1
    SET(PORTB, 1); // STEP MOTOR 2
    CLR(PORTD, 5);
    CLR(PORTB, 1);
Заметно увеличилось быстродействие, "предельный" коэффициент KP (при котором он начинает автоколебания с увеличивающейся амплитудой) теперь почти в 2 раза меньше. Но на результат никак не повлияло.
4. Я удалил этот цикл. Считал данные так, как это сделано в оригинальном коде B-robot. Это вообще никак не повлияло на работу.
mpu.getFIFOBytes(fifoBuffer, 16); // We only read the quaternion
mpu.dmpGetQuaternion(&q, fifoBuffer);
Reference URL's