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.

Please help me to discover my mistake and suggest for further steps.

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((a*a) + (b*b))

def y_rotation(x, y, z):

radians = math.atan2(x, distance(y, z))

return -math.degrees(radians)

def x_rotation(x, y, z):

radians = math.atan2(y, distance(x, z))

return math.degrees(radians)

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)
```