I have built a self balancing robot using raspberry pi 3b+. I am not able to reduce the response time. Hence, the robot doesn't balance properly. I am using MPU 6050, 150rpm 12V DC motor. I checked mpu 6050 and the motors individually and they seem to be working fine. The code also works. But, since the response time is more, the robot falls to the ground.
Please tell me what is wrong with my code.


import sys
import time
import RPi.GPIO as GPIO
import smbus
import math


# Power management registers
power_mgmt_1 = 0x6b
power_mgmt_2 = 0x6c

bus = smbus.SMBus(1) # or bus = smbus.SMBus(1) for Revision 2 boards
address = 0x68       # This is the address value read via the i2cdetect command

#motor pins initialization
GPIO.setup(Forward1, GPIO.OUT)
GPIO.setup(Backward1, GPIO.OUT)
GPIO.setup(Forward2, GPIO.OUT)
GPIO.setup(Backward2, GPIO.OUT)

#motor forward
def forward(x):
    GPIO.output(Forward1, GPIO.HIGH)
    GPIO.output(Forward2, GPIO.HIGH)
    print("Moving Forward")
    GPIO.output(Forward1, GPIO.LOW)
    GPIO.output(Forward2, GPIO.LOW)

#motor reverse
def reverse(x):
    GPIO.output(Backward1, GPIO.HIGH)
    GPIO.output(Backward2, GPIO.HIGH)
    print("Moving Backward")
    GPIO.output(Backward1, GPIO.LOW)
    GPIO.output(Backward2, GPIO.LOW)

# mpu6050 read 
def read_byte(adr):
    return bus.read_byte_data(address, adr)

def read_word(adr):
    high = bus.read_byte_data(address, adr)
    low = bus.read_byte_data(address, adr+1)
    val = (high << 8) + low
    return val

def read_word_2c(adr):
    val = read_word(adr)
    if (val >= 0x8000):
        return -((65535 - val) + 1)
        return val

def dist(a,b):
    return math.sqrt((a*a)+(b*b))

def get_y_rotation(x,y,z):
    radians = math.atan2(x, dist(y,z))
    return -math.degrees(radians)

def get_x_rotation(x,y,z):
    radians = math.atan2(y, dist(x,z))
    return math.degrees(radians)

def get_direction():

    # Now wake the 6050 up as it starts in sleep mode
    bus.write_byte_data(address, power_mgmt_1, 0)

    print("gyro data")

    gyro_xout = read_word_2c(0x43)
    gyro_yout = read_word_2c(0x45)
    gyro_zout = read_word_2c(0x47)

    print("gyro_xout: ", gyro_xout, " scaled: ", (gyro_xout / 131))
    print("gyro_yout: ", gyro_yout, " scaled: ", (gyro_yout / 131))
    print("gyro_zout: ", gyro_zout, " scaled: ", (gyro_zout / 131))

    print("accelerometer data")

    accel_xout = read_word_2c(0x3b)
    accel_yout = read_word_2c(0x3d)
    accel_zout = read_word_2c(0x3f)

    accel_xout_scaled = accel_xout / 16384.0
    accel_yout_scaled = accel_yout / 16384.0
    accel_zout_scaled = accel_zout / 16384.0

    #print("accel_xout: ", accel_xout, " scaled: ", accel_xout_scaled)
    #print("accel_yout: ", accel_yout, " scaled: ", accel_yout_scaled)
    #print("accel_zout: ", accel_zout, " scaled: ", accel_zout_scaled)

    print("x rotation: " , get_x_rotation(accel_xout_scaled, accel_yout_scaled, accel_zout_scaled))
    print("y rotation: " , get_y_rotation(accel_xout_scaled, accel_yout_scaled, accel_zout_scaled))
    return get_y_rotation(accel_xout_scaled, accel_yout_scaled, accel_zout_scaled)

# while loop begins
margin = 7
    while (1):
        dir = get_direction()
        if(dir < -margin):
            print("move forward")
        elif(dir > margin):
            print("move reverse")

    print("Some error")

  • First problem that comes to mind is that it's in Python. If speed is ultra-important, then you'll probably need to go to C or even assembler. Before you start mucking with that, however, I think you need to get a requirement on the timing and figure out how it differs from the execution time of your code. You may need different HW, not just different software. – Brick Mar 21 at 16:48
  • why would you have code like this in a program that you want to speed up? ... time.sleep(x) – jsotola Mar 21 at 18:21
  1. The code is written in Python which is an interpreted language. It will be somewhat slower than a compiled language such as C.
  2. You have many print statements in the code. That will slow the main loop considerably.

We don't know anything about your robot's size or shape, and that could matter. Some shapes are easier to balance, for example, and therefore probably less sensitive to control timing delays. Balance is often time-sensitive, however, so I'm working under that assumption for what follows.

You have several potential issues here. Depending on your situation, some may be ok to ignore, some manageable, and some fatal to your current design. You'll have to figure that out or give us more information to help you:

  • The Raspberry Pi is running a general operating system (i.e. Linux), which means that your code, no matter what language or how well written, may not run continuously as Linux schedules time on the CPU across everything that is running. If you need very high speed, you may not be able to use the Pi at all. You'll need a dedicated microcontroller or custom hardware, although those could be slaved to a Raspberry Pi running higher logic.
  • Python is not suitable for time-sensitive applications. If you really need speed, you'll need C or possibly assembly.
  • Manipulating strings, including writing to standard output, takes a relatively large number of processing cycles. You'll probably want to minimize or eliminate those, especially the ones in the loop. (The ones at start-up, which only run once, probably don't matter.)
  • If you are going to run on the Pi, you may need to go to multi-threading or to use interrupts to get the responsiveness that you need.

To figure out what of this matters and what does not, you'll need to get an idea of your actual requirements for timing and then start to estimate whether the Pi can do it and/or to measure the speed of your code via profiling tools.


Brick's answer is already very comprehensive, but I would like to add a couple of things:

  • time.sleep is a blocking function, which implies that, at each loop, before you perform a new calculation in your thread to adjust the robot's position, you need to wait the specified amount of time in your sleep function. Besides, sleep only guarantees precision up to a certain point, which depending on your Operating System may not enough.
  • Python might be more suitable to test out your algorithm (to see if you can get the expected numerical results) but it will not be as fast as C.
  • Furthermore, you may want to try another platform that allows for a more predictable system, such as a microcontroller that doesn't need an Operating System to run.

You mentioned the code works, but what exactly do you mean by this? Did it give you the expected numerical results? And even then, control systems require very fine-tuning depending on your application. Even if your code does produce the numbers that you want, your control system will only be as good as your model. So my final suggestion is that, before you try to change or improve the time response of your Python code, review your model, and then your numerical results.

  • By "the code works", I mean that, the motor exactly how it is supposed to work. It moves forward when inclined forward and backward when inclined backward. But, once it detects an imbalance, the motor takes a while to move like it is supposed to. – ece12345 Mar 22 at 10:57
  • @ece12345, yes, but that doesn't necessarily mean that the code is producing the correct adjustment, regardless of the timing behavior. How do you know it's moving forward enough for it to be balance. Assuming it were all fast enough, how do you know your control system works. – David Mar 22 at 11:00
  • I missed the time.sleep call. That's clearly an issue too if you want to go for speed! +1. Regarding the reply from the OP in comments that the motor takes a while to adjust, requiring the sleep, that goes into the general category of issues that I mentioned originally that you may need different hardware for this. If your motor responds too slowly, then it doesn't matter how fast you make your software. – Brick Mar 22 at 13:37
  • 1
    Putting 0.1s and 9.8m/s² into s = u.t + ½a.t² gives 49mm. So the centre of gravity of your robot could drop about 5cm/two inches between your sleeps. You probably want to continually PWM your motor at a speed and direction based on some function of the acceleration (PID or Kalman), not bang-bang control it and let it fall that much. – Pete Kirkham Mar 22 at 16:21

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.