Asked
Viewed 19 times
-1
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']
#print(gyroZ)
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)
print(rotation_z)
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))
sleep(0.2)
New contributor
-
Hi @ a masri, Welcome and nice to meet you. Ah, let me see. So you PID thing gives garbage results. There are many causes: (1) MPU6050 6-DOF’s Accelero or Gyro or both gives garbage reads, ie GIGO – Garbage In, Garbage out, so it might not be the PID guy’s fault. To troubleshoot, I would suggest to first check out if accelero or gyro is the bad guy. I would suggest to first check out accelero, which is usually simpler. You might like to show us your accelero readings to see how garbage it is. This is a accelero only Q&A you might like to read. – tlfong01 1 hour ago
-
You can see that the OP’s and my accelero readings is also garbage, and we tried different get around to improve the results: raspberrypi.stackexchange.com/questions/107187/… – tlfong01 1 hour ago
-
The reference above is not using MPU6050. In case you are interested to see how MPU6050 can be setup and how to do basic testing, read write registers and read basic Gx, Gy, Gz values, you can read the following: (1) raspberrypi.stackexchange.com/questions/105071/… (2) raspberrypi.stackexchange.com/questions/74865/… – tlfong01 10 mins ago
-
(3) raspberrypi.org/forums/… (4) raspberrypi.org/forums/… (5) raspberrypi.org/forums/… (6) raspberrypi.org/forums/… (7) raspberrypi.org/forums/…. – tlfong01 10 mins ago
-
The above references might be out of date, and sample python programs/drivers might not work on Rpi4B buster. In case you get stuck in Rpi4B python programs, feel free to let me know, and I will see if I can reproduce your problem in my Rpi4B. You might also like to let me know the PID tutorial you are following, and I can see if I can help there. – tlfong01 7 mins ago
-
Or are you following any of the following tutorials? (1) pypi.org/project/pid_controller (2) pypi.org/project/simple-pid (3) github.com/korfuri/PIDController (4) onion.io/2bt-pid-control-python (5) chiefdelphi.com/t/…. PS – Ref (2) SimplePID0.2.4 looks good for newbies, but I have not tried it on my Rpi4B Python 3.7.3. – tlfong01 2 mins ago Edit
.END
Categories: Uncategorized