Self balancing robot - how to obtain the elapsed angle from gyroscopic data?

Question

I am trying to complete my University project building and programming a self balancing robot in python (using RPi) but have hit a road block in my knowledge.

From the MPU6050 I am able to obtain both acceleration and gyroscopic data, furthermore the angles from them (using atan2 function for accel angles and integrating dt for gyro angles). However I cannot think of a way to code this data so that in a loop of given time (dt) it will turn the change in angle (from gyro data) into the total elapsed angle.

I need to be able to loop the program so that it will run for time (dt), store this data, run the program again but expect + or - the previous stored data. I will then be able to use both angles from the gyro and accel data and run it through a complimentary filter.

I have uploaded the relevant code below.

import smbus                    #import SMBus module of I2C
import time
from time import sleep
import math

#some MPU6050 Registers and their Address
PWR_MGMT_1   = 0x6B
SMPLRT_DIV   = 0x19
CONFIG       = 0x1A 
GYRO_CONFIG  = 0x1B
INT_ENABLE   = 0x38
ACCEL_XOUT_H = 0x3B
ACCEL_YOUT_H = 0x3D
ACCEL_ZOUT_H = 0x3F
GYRO_XOUT_H  = 0x43
GYRO_YOUT_H  = 0x45
GYRO_ZOUT_H  = 0x47

DT = 0.02
rad_to_deg = 180/(math.pi)



def MPU_Init():
    #write to sample rate register
    bus.write_byte_data(Device_Address, SMPLRT_DIV, 7)
    
    #Write to power management register
    bus.write_byte_data(Device_Address, PWR_MGMT_1, 1)
    
    #Write to Configuration register
    bus.write_byte_data(Device_Address, CONFIG, 0)
    
    #Write to Gyro configuration register
    bus.write_byte_data(Device_Address, GYRO_CONFIG, 24)
    
    #Write to interrupt enable register
    bus.write_byte_data(Device_Address, INT_ENABLE, 1)

def read_raw_data(addr):
    #Accelero and Gyro value are 16-bit
        high = bus.read_byte_data(Device_Address, addr)
        low = bus.read_byte_data(Device_Address, addr+1)
    
        #concatenate higher and lower value
        value = ((high << 8) | low)
        
        #to get signed value from mpu6050
        if(value > 32768):
                value = value - 65536
        return value


bus = smbus.SMBus(1)    # or bus = smbus.SMBus(0) for older version boards
Device_Address = 0x68   # MPU6050 device address
MPU_Init()

print (" Reading Data of Gyroscope and Accelerometer")


    
while True:
        #Read Accelerometer raw value
        acc_x = read_raw_data(ACCEL_XOUT_H)
        acc_y = read_raw_data(ACCEL_YOUT_H)
        acc_z = read_raw_data(ACCEL_ZOUT_H)
        
        #Read Gyroscope raw value
        gyro_x = read_raw_data(GYRO_XOUT_H)
        gyro_y = read_raw_data(GYRO_YOUT_H)
        gyro_z = read_raw_data(GYRO_ZOUT_H)
        
        #Full scale range +/- 250 degree/C as per sensitivity scale factor
        Ax = acc_x/16384
        Ay = acc_y/16384
        Az = acc_z/16384
        
        Gx = gyro_x/131.0
        Gy = gyro_y/131.0
        Gz = gyro_z/131.0
        
        gyro_x_angle= Gy * DT 

        accel_x_angle = (math.atan2(Az,Ax)-(math.pi/2))*rad_to_deg

        
time.sleep(DT)

there seems to be many similar self balancing robot projects but very few programmed in python that i can find Any help would be much appreciated.

I have experimented with lists, for loops, while loops but cannot seem to obtain the required results.

Self balancing robot - how to obtain the elapsed angle from gyroscopic data?

0 Answers0