Self-balancing robot using Raspberry 3b+

Question

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.

CODE:

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

mode=GPIO.getmode()
print(mode)
#GPIO.cleanup()
print("Start")
Forward1=38
Backward1=37
Forward2=36
Backward2=35
sleeptime=0.1

# 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.setmode(GPIO.BOARD)
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")
    time.sleep(x)
    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")
    time.sleep(x)
    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)
    else:
        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")
    print("---------")

    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("")
    print("accelerometer data")
    print("------------------")

    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
try:
    while (1):
        dir = get_direction()
        if(dir < -margin):
            print("move forward")
            forward(sleeptime)
        elif(dir > margin):
            print("move reverse")
            reverse(sleeptime)

except:
    print("Some error")

GPIO.cleanup()

Answer 1

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.

Answer 2

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.

Answer 3

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.