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((aa) + (bb))
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)