PID MPU6050 notes

Viewed 19 times

I have a car robot 4 wheel the car do not going straight, who to set up a pid for going straight using mpu6050

I’m trying to use this code but do not get a accurate result

from mpu6050 import mpu6050
from time import sleep
import math
from pidcontroller import PIDController

sensor = mpu6050(0x68)
K = 0.98
K1 = 1 - K

time_diff = 0.02
ITerm = 0

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']

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)

def z_rotation(x, y, z):
    radians = math.atan2(z, distance(x, y))
    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_offset_z = gTempZ

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

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
    gyroZ -= gyro_offset_z

    gyro_x_delta = (gyroX * time_diff)
    gyro_y_delta = (gyroY * time_diff)
    gyro_z_delta = (gyroZ * 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)
    rotation_z = z_rotation(accelX, accelY, accelZ)
    last_x = K * (last_x + gyro_z_delta) + (K1 * rotation_z)

    PID = PIDController(P=5.0, I=0.5, D=0.0)
    PIDx = PID.step(last_x)

    print(int(last_x), 'PID: ', int(PIDx))
 New contributor

Categories: Uncategorized

Leave a Reply

Fill in your details below or click an icon to log in:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out /  Change )

Google photo

You are commenting using your Google account. Log Out /  Change )

Twitter picture

You are commenting using your Twitter account. Log Out /  Change )

Facebook photo

You are commenting using your Facebook account. Log Out /  Change )

Connecting to %s

This site uses Akismet to reduce spam. Learn how your comment data is processed.

%d bloggers like this: