Python Based Self-Balancing Robot using MPU6050

[beginnerPi]

Hello Everybody,

I am trying to build a Self-Balancing Robot ( nothing new ) but i am trying to build this Robot with Python Code . I am half way gone , i think . My robot is moving back and forth but it fails to maintain the balance . I am doing some wrong calculation on the PID and Accelerometer Angle , i think but didn’t find out the Error . I need the community Help

The Parts I am using is :
1. RPi B+
2. GY-521 MPU-6050 Module 3 axial gyroscope accelerometer stance tilt http://www.amazon.com/Kootek-Arduino-MP ... B008BOPN40
3. 2 DC Motor 12v 600RPM http://www.dfrobot.com/index.php?route= ... uct_id=354
4. 2 Wheels http://www.dfrobot.com/index.php?route= ... uct_id=353
5. L298N Dual H-Bridge Motor Control & Drives Module http://www.amazon.com/gp/product/B00CAG6GX2

Robotics is kind a hobby and i bought Raspberry Pi B+ few weeks ago , since it is very hard to get in my country. I am trying to google and read about the whole process and theory behind the self-balancing robot and also reading code ( I found c and c++ mostly ) that i can not fully understand . I love python and i am trying to write the code for this robot in Python only . I am also having trouble doing the PWM for the DC motor i choose

Here is the full Source Code
https://github.com/thebdcoder/balancebot.git

I have managed to get the MPU6050 Library from
https://github.com/bitify/raspi/tree/ma ... on/sensors
I also Found a PID library for Python and using it . Saw many user using it for Quadcopter Project

All those Library are in the 6050 folder in the Git Repository

Here is the final Code

Code: Select all

#!/usr/bin/python

import smbus
import math
import time
from MPU6050 import MPU6050
from PID import PID
import motor as MOTOR

gyro_scale = 131.0
accel_scale = 16384.0
RAD_TO_DEG = 57.29578
M_PI = 3.14159265358979323846

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

now = time.time()

K = 0.98
K1 = 1 - K

time_diff = 0.01

sensor = MPU6050(bus, address, "MPU6050")
sensor.read_raw_data()	# Reads current data from the sensor

rate_gyroX = 0.0
rate_gyroY = 0.0
rate_gyroZ = 0.0

gyroAngleX = 0.0 
gyroAngleY = 0.0 
gyroAngleZ = 0.0 

raw_accX = 0.0
raw_accY = 0.0
raw_accZ = 0.0

rate_accX = 0.0
rate_accY = 0.0
rate_accZ = 0.0

accAngX = 0.0

CFangleX = 0.0
CFangleX1 = 0.0

K = 0.98

FIX = -12.89

#print "{0:.4f} {1:.2f} {2:.2f} {3:.2f} {4:.2f} {5:.2f} {6:.2f}".format( time.time() - now, (last_x), gyro_total_x, (last_x), (last_y), gyro_total_y, (last_y))

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)

p=PID(1.0,-0.04,0.0)
p.setPoint(0.0)

for i in range(0, int(300.0 / time_diff)):
    time.sleep(time_diff - 0.005) 
    
    sensor.read_raw_data()
    
    # Gyroscope value Degree Per Second / Scalled Data
    rate_gyroX = sensor.read_scaled_gyro_x()
    rate_gyroY = sensor.read_scaled_gyro_y()
    rate_gyroZ = sensor.read_scaled_gyro_z()
	
    # The angle of the Gyroscope
    gyroAngleX += rate_gyroX * time_diff 
    gyroAngleY += rate_gyroY * time_diff 
    gyroAngleZ += rate_gyroZ * time_diff 

    # Accelerometer Raw Value
    raw_accX = sensor.read_raw_accel_x()
    raw_accY = sensor.read_raw_accel_y()
    raw_accZ = sensor.read_raw_accel_z()
    
    # Accelerometer value Degree Per Second / Scalled Data
    rate_accX = sensor.read_scaled_accel_x()
    rate_accY = sensor.read_scaled_accel_y()
    rate_accZ = sensor.read_scaled_accel_z()
    
    # http://ozzmaker.com/2013/04/18/success-with-a-balancing-robot-using-a-raspberry-pi/
    accAngX = ( math.atan2(rate_accX, rate_accY) + M_PI ) * RAD_TO_DEG
    CFangleX = K * ( CFangleX + rate_gyroX * time_diff) + (1 - K) * accAngX
    
    # http://blog.bitify.co.uk/2013/11/reading-data-from-mpu-6050-on-raspberry.html 
    accAngX1 = get_x_rotation(rate_accX, rate_accY, rate_accX)
    CFangleX1 = ( K * ( CFangleX1 + rate_gyroX * time_diff) + (1 - K) * accAngX1 )
    
    # Followed the Second example because it gives resonable pid reading
    pid = p.update(CFangleX1)
    
    if(pid > 0):
        MOTOR.forward(pid)
    else:
        MOTOR.backward( (pid*-1) )
    
    print "{0:.2f} {1:.2f} {2:.2f} {3:.2f} | {4:.2f} {5:.2f} | {6:.2f}".format( gyroAngleX, gyroAngleY , accAngX, CFangleX, accAngX1, CFangleX1, pid)

Here is some sample output of this Code :
gyroAngleX -35.93 gyroAngleY 18.26 accAngX 67.76 CFangleX 118.11 | accAngX1 -16.12 CFangleX1 -19.99 | pid : -0.01

Code: Select all

-35.93 18.26 67.76 118.11 | -16.12 -19.99 | -0.01
-35.40 18.26 68.50 117.64 | -15.57 -19.38 | -0.62
-34.86 18.25 101.47 117.84 | 8.16 -18.31 | -1.69
-34.31 18.25 99.83 118.03 | 6.98 -17.26 | -2.74
-33.75 18.25 137.20 118.96 | 37.36 -15.61 | -4.39
-33.19 18.26 145.63 120.04 | 45.95 -13.83 | -6.17
-32.61 18.26 153.73 121.28 | 55.08 -11.89 | -8.11
-32.02 18.26 165.55 122.74 | 69.98 -9.67 | -10.33
-31.42 18.26 44.88 121.77 | -35.38 -9.61 | -10.39
-31.05 18.27 185.88 123.42 | 81.72 -7.41 | -12.59
-30.59 18.29 83.42 123.07 | -4.66 -6.91 | -13.09
-30.18 18.30 28.81 121.59 | -52.12 -7.41 | -12.59
-29.77 18.30 140.66 122.37 | 40.78 -6.04 | -13.96
-29.36 18.30 89.12 122.11 | -0.62 -5.53 | -14.47
-28.96 18.30 45.39 120.96 | -34.90 -5.73 | -14.27
-28.56 18.30 166.50 122.26 | 71.24 -3.80 | -16.20
-28.11 18.31 163.19 123.53 | 66.87 -1.94 | -18.06
-27.75 18.33 51.13 122.43 | -29.68 -2.15 | -17.85
-27.40 18.35 147.70 123.28 | 48.21 -0.80 | -19.20
-27.07 18.37 25.34 121.65 | -56.20 -1.58 | -18.42
-26.73 18.38 178.36 123.11 | 87.68 0.54 | -20.52
-26.45 18.42 18.66 121.30 | -64.48 -0.49 | -19.51
-26.21 18.45 317.37 125.45 | -37.53 -0.99 | -19.01
-26.00 18.49 191.33 126.98 | 74.19 0.72 | -20.69
-25.85 18.47 161.86 127.82 | 65.14 2.15 | -22.03
-25.76 18.47 185.21 129.06 | 82.65 3.85 | -23.58
-25.68 18.45 176.29 130.08 | 84.76 5.55 | -25.06
-25.65 18.43 171.29 130.93 | 77.77 7.02 | -26.25
-25.75 18.43 172.08 131.66 | 78.86 8.36 | -27.26
-25.90 18.43 162.56 132.13 | 66.05 9.37 | -27.89
-26.11 18.43 140.49 132.09 | 40.61 9.78 | -27.91
-26.33 18.43 187.97 132.99 | 78.80 10.95 | -28.64
-26.58 18.48 128.25 132.66 | 29.13 11.07 | -28.32
-26.88 18.55 168.54 133.08 | 74.00 12.03 | -28.80
-27.17 18.59 177.89 133.69 | 87.01 13.25 | -29.49
-27.48 18.62 170.87 134.12 | 77.19 14.22 | -29.89
-27.85 18.63 163.88 134.36 | 67.77 14.94 | -30.00
-28.23 18.64 169.18 134.68 | 74.88 15.76 | -30.20
-28.64 18.68 178.51 135.16 | 87.90 16.80 | -30.57
-29.11 18.72 174.15 135.48 | 81.75 17.64 | -30.70
-29.59 18.73 161.26 135.52 | 64.37 18.10 | -30.44
-30.10 18.73 176.95 135.86 | 85.70 18.96 | -30.54
-30.67 18.73 168.62 135.95 | 74.11 19.50 | -30.30
-31.27 18.73 167.82 136.00 | 73.03 19.98 | -29.98
-31.88 18.75 169.39 136.07 | 75.16 20.49 | -29.67
-32.53 18.77 170.35 136.12 | 76.48 20.97 | -29.31
-33.20 18.79 182.87 136.39 | 85.95 21.61 | -29.09
-33.89 18.80 172.46 136.44 | 79.40 22.09 | -28.68
-34.59 18.80 111.92 135.26 | 15.89 21.28 | -27.02
-35.29 18.83 183.87 135.55 | 84.53 21.86 | -26.73
-36.01 18.85 169.86 135.53 | 75.81 22.23 | -26.21
-36.74 18.87 98.80 134.09 | 6.25 21.20 | -24.33
-37.48 18.91 139.77 133.47 | 39.89 20.85 | -23.15
-38.16 18.91 18.53 130.51 | -64.64 18.47 | -20.03
-38.89 18.96 55.18 128.29 | -26.19 16.87 | -17.75
-39.59 18.98 106.23 127.16 | 11.63 16.07 | -16.31
-40.28 19.01 33.70 124.61 | -46.67 14.15 | -13.82
-40.99 19.02 9.04 121.60 | -77.32 11.61 | -10.82
-41.72 19.01 144.39 121.34 | 44.63 11.56 | -10.31
-42.43 19.02 178.07 121.79 | 87.27 12.38 | -10.63
-43.12 19.04 86.89 120.41 | -2.20 11.42 | -9.21
-43.83 19.06 24.05 117.79 | -57.75 9.33 | -6.76
-44.60 19.07 38.60 115.45 | -41.53 7.56 | -4.68
-45.41 19.08 21.05 112.76 | -61.45 5.39 | -2.29
-46.28 19.10 19.55 110.05 | -63.34 3.16 | 0.06
-47.19 19.10 168.62 110.32 | 74.11 3.68 | -0.31
-47.92 19.14 147.37 110.36 | 47.84 3.86 | -0.33
-48.69 19.16 347.73 114.35 | -72.91 1.56 | 2.02
-49.44 19.18 44.87 112.22 | -35.39 0.09 | 3.50

I am using CFangleX1 for PID . I have used Complimentary Filter for CFangleX1 angle

I am not sure the angle i am getting is quite right and also the pid calculation . This is my first project .
One more challenge i am facing is controlling Motor Speed using Python GPIO.PWM .
The approach i am following is :

Code: Select all

GPIO.setup(21, GPIO.OUTPUT)
GPIO.PWM(21, 100)
GPIO.start(PID) # the Duty Cycle between ( 1 to 100 )

This working so far but not quite sure that the PID is the DutyCycle that i sould send . I saw in many Cpp code that it sends the output between -255 to 255 . What i am doing for python is if pid is positive then move forward else backward ( if backward then move backward but convert pid/speed to positive (pid * -1) because it can only take int between 0 to 100 ) .

The Article i am reading on and on is :
http://letsmakerobots.com/node/38610
http://www.instructables.com/id/Self-Ba ... /?ALLSTEPS
http://blog.bitify.co.uk/2013/11/using- ... mbine.html
http://ozzmaker.com/2013/04/18/success- ... pberry-pi/

They are all Good but none of them Provide Sample Output . Some article with Source Code But like I Said all in .ino ( C & Cpp )
I Really need to solve it . I hope you understand the point and if not let me know which . I am waiting for the help of this community .

[beginnerPi]

Here is the Current Structure/Frame of this Robot

http://postimg.org/image/rvf45zvsp/

Also note that i am not using Battery at this moment but using 12v DC power Supply for Motor

[joan]

You are using software timed PWM which may be part of the problem.

[beginnerPi]

So anything i can do with Python and for DC motor for PWM ?

[joan]

So anything i can do with Python and for DC motor for PWM ?

There are several options.

• There are two hardware PWM channels available on the B+ (gpios 12/13 or gpios 18/19 or a combination thereof).
• There are several software modules which provide hardware (DMA) timed PWM on all the gpios, i.e.
  • RPIO.GPIO
  • servoblaster
  • pi-blaster
  • my own pigpio

[beginnerPi]

Well in GPIO.BCM mode the Pin 19 and 18 have this function . And for 2 Dc motor going back and forth i guess i need 4 pins with hardware (DMA) timed PWM . And the code/library you included is in C any thing i can do with Pythion GPIO library . I am already using Python GPIO.PWM()

And any comment on should i directly use PID output for PWM or trim , ... ?? . Any thing wrong in my Angle Calculation using MPU6050

Code: Select all

# Gyroscope value Degree Per Second / Scalled Data
    rate_gyroX = sensor.read_scaled_gyro_x()
    rate_gyroY = sensor.read_scaled_gyro_y()
    rate_gyroZ = sensor.read_scaled_gyro_z()
   
    # The angle of the Gyroscope
    gyroAngleX += rate_gyroX * time_diff
    gyroAngleY += rate_gyroY * time_diff
    gyroAngleZ += rate_gyroZ * time_diff

    # Accelerometer Raw Value
    raw_accX = sensor.read_raw_accel_x()
    raw_accY = sensor.read_raw_accel_y()
    raw_accZ = sensor.read_raw_accel_z()
   
    # Accelerometer value Degree Per Second / Scalled Data
    rate_accX = sensor.read_scaled_accel_x()
    rate_accY = sensor.read_scaled_accel_y()
    rate_accZ = sensor.read_scaled_accel_z()
   
    # http://ozzmaker.com/2013/04/18/success-with-a-balancing-robot-using-a-raspberry-pi/
    accAngX = ( math.atan2(rate_accX, rate_accY) + M_PI ) * RAD_TO_DEG
    CFangleX = K * ( CFangleX + rate_gyroX * time_diff) + (1 - K) * accAngX
   
    # http://blog.bitify.co.uk/2013/11/reading-data-from-mpu-6050-on-raspberry.html
    accAngX1 = get_x_rotation(rate_accX, rate_accY, rate_accX)
    CFangleX1 = ( K * ( CFangleX1 + rate_gyroX * time_diff) + (1 - K) * accAngX1 ) 

[joan]

Well in GPIO.BCM mode the Pin 19 and 18 have this function . And for 2 Dc motor going back and forth i guess i need 4 pins with hardware (DMA) timed PWM . And the code/library you included is in C any thing i can do with Pythion GPIO library . I am already using Python GPIO.PWM()
...

RPIO.GPIO is a Python module. All the others are usable from Python and may be controlled by writing to a pipe. In addition pigpio has a Python module.

Can't comment on the PID.

[Mark_T]

When implementing a servomotor, which is what a PID loop controlling a motor is,
you are wise to avoid using slow decay mode, and ensuring the PWM frequency
is high enough.

The best set up is to use synchronous rectification (like fast decay except
the bridge is always driven, PWM reverses the direction, so that 50% PWM = stationary,
0% is backwards, 100% is forwards). This mode is lossier (magnetic losses
in the motor), but better behaved at stationary (which is what you want for balance).

Because of the need for relatively high PWM frequency you may find the L298 is
just too slow. A MOSFET bridge may proof more capable. However it all depends
on the motor and how loaded it is.

[beginnerPi]

When implementing a servomotor, which is what a PID loop controlling a motor is,
you are wise to avoid using slow decay mode, and ensuring the PWM frequency
is high enough.

The best set up is to use synchronous rectification (like fast decay except
the bridge is always driven, PWM reverses the direction, so that 50% PWM = stationary,
0% is backwards, 100% is forwards). This mode is lossier (magnetic losses
in the motor), but better behaved at stationary (which is what you want for balance).

Because of the need for relatively high PWM frequency you may find the L298 is
just too slow. A MOSFET bridge may proof more capable. However it all depends
on the motor and how loaded it is.

Yes i noticed its a bit slow ..but i have seen people achieving this using L298 and MPU6050 with Pi . What am i missing here ... ? I have included the code ..

[Mark_T]

A slow bridge limits your maximum PWM frequency - not an issue for larger motors, but will start to
hit you with smaller / less inductive motors. Even if the motor drive isn't briliant a PID loop can function,
it might just be more jittery or fiddly to configure.

[joan]

The PWM frequency is irrelevant to your problem. The majority of Arduinos seem to run at about 500 Hz. The Pi can easily meet that. There must be thousands of Arduino based self-balancing robots.

PIDs need tuning. It does not appear to be a simple exercise.

[venkyskar]

Dude i am planning to run motor according to the readings of MPU6050 can you help me with the code

[mwilliams03]

I see you have linked one of my balancing robots. I have built two, here is what I learnt;
-C is better than python, as a well timed loop is need to get less drift on the gyro, C is also faster.
-Motors with too much backlash will be harder to balance. Motors with planetary gears are better
-Get an I2C motor controller to control the wheels. (MD22) Then you don't have to worry about PWM
-Try and work out a way to change the PID values while balancing. I used an Adafruit RF receiver to change the values on the fly; https://www.adafruit.com/products/1096
-Try balancing on carpet first
-Make sure your motor has enough torque.
-place the IMU at the bottom near the wheel axle.


Question, is it balancing at all? Even a little? maybe you can show a video?

[venkyskar]

I see you have linked one of my balancing robots. I have built two, here is what I learnt;
-C is better than python, as a well timed loop is need to get less drift on the gyro, C is also faster.
-Motors with too much backlash will be harder to balance. Motors with planetary gears are better
-Get an I2C motor controller to control the wheels. (MD22) Then you don't have to worry about PWM
-Try and work out a way to change the PID values while balancing. I used an Adafruit RF receiver to change the values on the fly; https://www.adafruit.com/products/1096
-Try balancing on carpet first
-Make sure your motor has enough torque.
-place the IMU at the bottom near the wheel axle.


Question, is it balancing at all? Even a little? maybe you can show a video?

can you help me with the changes in the program so that the bot can run according to the change in angle i mean if i want to build a Accelerometer controlled robot

[pulak]

sir can you please help me to setup the mpu6050 with raspberry pi 3b

[kshparis]

sir can you please help me to setup the mpu6050 with raspberry pi 3b

http://www.electronicwings.com/raspberr ... spberry-pi