# Self Balancing Robot - Raspberry PI 4 & Mpu6050

Hey All,

I would like to build new self balancing robot with ODrive using Raspberry PI4 and MPU6050.
I manage to control tilt with gyro sensor but while the PID equal to zero motors keep spinning and not stopped.
Im using the code below:

from mpu6050 import mpu6050
from time import sleep
import math
from pidcontroller import PIDController
import RPi.GPIO as GPIO
import odrive
from odrive.enums import *

print(“finding an odrive…”)
my_drive = odrive.find_any()

print("Bus voltage is " + str(my_drive.vbus_voltage) + “V”)

my_drive.axis0.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL

my_drive.axis1.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL

#This backward function takes a velocity argument that is the PID value. Both motors drives backward
def backward(velocity):
my_drive.axis0.controller.input_vel = 2
my_drive.axis1.controller.input_vel = 2

#Alike the backward funtion this forward function does the same thing but moves both the motors forward.
def forward(velocity):
my_drive.axis0.controller.input_vel = -2
my_drive.axis1.controller.input_vel = -2

#If the PID value is 0 (the Robot is ‘balanced’) it uses this equilibrium function.
def equilibrium():
my_drive.axis0.controller.input_vel = 0
my_drive.axis1.controller.input_vel = 0
my_drive.axis0.controller.input_vel = AXIS_STATE_IDLE
my_drive.axis1.controller.input_vel = AXIS_STATE_IDLE

sensor = mpu6050(0x68)
#K and K1 → Constants used with the complementary filter
K = 0.98
K1 = 1 - K

time_diff = 0.02
ITerm = 0

#Calling the MPU6050 data
accel_data = sensor.get_accel_data()
gyro_data = sensor.get_gyro_data()

aTempX = accel_data[‘x’]
aTempY = accel_data[‘y’]
aTempZ = accel_data[‘z’]

gTempX = gyro_data[‘x’]
gTempY = gyro_data[‘y’]
gTempZ = gyro_data[‘z’]

#some math
def distance(a, b):
return math.sqrt((aa) + (bb))

def y_rotation(x, y, z):

def x_rotation(x, y, z):

last_x = x_rotation(aTempX, aTempY, aTempZ)
last_y = y_rotation(aTempX, aTempY, aTempZ)

gyro_offset_x = gTempX
gyro_offset_y = gTempY

gyro_total_x = (last_x) - gyro_offset_x
gyro_total_y = (last_y) - gyro_offset_y

#the so called ‘main loop’ that loops around and tells the motors wether to move or not
while True:
accel_data = sensor.get_accel_data()
gyro_data = sensor.get_gyro_data()

``````accelX = accel_data['x']
accelY = accel_data['y']
accelZ = accel_data['z']

gyroX = gyro_data['x']
gyroY = gyro_data['y']
gyroZ = gyro_data['z']

gyroX -= gyro_offset_x
gyroY -= gyro_offset_y

gyro_x_delta = (gyroX * time_diff)
gyro_y_delta = (gyroY * time_diff)

gyro_total_x += gyro_x_delta
gyro_total_y += gyro_y_delta

rotation_x = x_rotation(accelX, accelY, accelZ)
rotation_y = y_rotation(accelX, accelY, accelZ)

#Complementary Filter
last_x = K * (last_x + gyro_x_delta) + (K1 * rotation_x)

#setting the PID values. Here you can change the P, I and D values according to yiur needs
PID = PIDController(P=-60, I=90.0, D=60.0)
PIDx = PID.step(last_x)

#if the PIDx data is lower than 0.0 than move appropriately backward
if PIDx < 0.0:
backward(-float(PIDx))
#StepperFor(-PIDx)
#if the PIDx data is higher than 0.0 than move appropriately forward
elif PIDx > 0.0:
forward(float(PIDx))
#StepperBACK(PIDx)
#if none of the above statements is fulfilled than do not move at all
else:
equilibrium()

print(int(last_x), 'PID: ', float(PIDx))
sleep(0.02)``````

Hi,

Just one thing I noticed - not sure but it may very well be the issue. In your euilibrium function you first set the input speeds to 0 but then to AXIS_STATE_IDLE which is inappropriate as an input velocity. AXIS_STATE_IDLE is actually encoded as a 1 so that might be causing your movement.

Regards
Max