def pid_action(self, pidvalue):
PID = PIDController(P=-40, I=1.0, D=5.0)
PIDx = PID.step(pidvalue)
if PIDx < 0.0:
backward(-float(PIDx))
elif PIDx > 0.0:
forward(float(PIDx))
else:
equilibrium()
def mainLoop():
inputSudut = round(angleMeter.get_kalman_roll(), 0) + round(
calibrationValue, 0)
PID = PIDController(P, I, D) #eight
PIDx = round(PID.step(inputSudut), 1)
if inputSudut < -setPoint:
forward(-(PIDx))
# print("forward")
elif inputSudut > setPoint:
backward((PIDx))
# print("back")
else:
equilibrium()
# print("equi")
print(round(inputSudut, 2), 'PID: ', PIDx)
def main():
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']
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
while start:
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_y = K * (last_y + gyro_y_delta) + (K1 * rotation_y)
#setting the PID values. Here you can change the P, I and D values according to your needs
PID = PIDController(P=slider1.get(), I=slider2.get(), D=slider3.get())
PIDx = PID.step(last_y)
#if the PIDx data is lower than 0.0 than move appropriately backward
if PIDx < 0.0:
if (PIDx < -100):
PIDx = -100
backward(-float(PIDx))
#StepperFor(-PIDx)
#if the PIDx data is higher than 0.0 than move appropriately forward
elif PIDx > 0.0:
if (PIDx > 100):
PIDx = 100
forward(float(PIDx))
#StepperBACK(PIDx)
#if none of the above statements is fulfilled than do not move at all
else:
equilibrium()
#update GUI
root.update()
with open('P', 'r') as f:
kp = float(f.read())
with open('I', 'r') as f:
ki = float(f.read())
with open('D', 'r') as f:
kd = float(f.read())
PID = PIDController(P=kp, I=ki, D=kd)
# import pdb; pdb.set_trace()
try:
while True:
a, b, c = getMPUVals()
b = b - 1.5
PIDx = PID.step(b)
print(f'{b} {PIDx}')
if PIDx < 0.0:
#botMoveBackward(-map(-PIDx, 0, 1000, 40, 200))
botMoveBackward(PIDx)
elif PIDx > 0.0:
#botMoveForward(map(PIDx, 0, 1000, 40, 200))
botMoveForward(PIDx)
else:
botEquilibrium()
# m1.rotate(255)
# m2.rotate(255)
# time.sleep(0.001)
except:
pass
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)
last_y = K * (last_y + gyro_y_delta) + (K1 * rotation_y)
# last_y = last_y - 15
#setting the PID values. Here you can change the P, I and D values according to yiur needs
PID = PIDController(P=50, I=0.0, D=0.0)
# PIDx = PID.step(last_x)
PIDy = PID.step(last_y)
#if the PIDx data is lower than 0.0 than move appropriately backward
if PIDy < 0.0:
backward(float(PIDy))
#StepperFor(-PIDx)
#if the PIDx data is higher than 0.0 than move appropriately forward
elif PIDy > 0.0:
forward(float(PIDy))
#StepperBACK(PIDx)
#if none of the above statements is fulfilled than do not move at all
else:
equilibrium()
# print(last_x, 'PID: ', int(PIDx))
print(last_y, 'PID: ', int(PIDy))
gyro_data = sensor.euler #function to call raw gyroscope oriention data from IMU
gyroY = (gyro_data[1]) #variable created to only grab the y-axis gyro data
#variables for time for PID
time1 = time.process_time()
dt = time1 - last_time
last_time = time1
#Calling PIDcontroller class from pidcontroller, setting P, I, and D gains
PID = PIDController(P=10,
I=0.05,
D=0.09,
Time=dt,
lastI=lastI,
lastError=lastError)
PIDy, lastI, lastError = PID.step(gyroY)
if gyroY < -50: #if tile angle is greater than 50, shut off motors (dead-band)
equilibrium()
elif gyroY > 50: #if tile angle is greater than 50, shut off motors (dead-band)
equilibrium()
elif PIDy < 0: #if PID value is less than 0, drive motors backwards
backward(-PIDy)
elif PIDy > 0: #if PID value is more than 0, drive motors forwards
forward(PIDy)
elif gyroY == 0: #if tilt angle is 0 (balanced), shut off motors.
equilibrium()
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=-78.5, I=1.0, D=1.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: ', int(PIDx))
gx -= gyro_offset_x
gy -= gyro_offset_y
gx_delta = (gx * time_diff)
gy_delta = (gy * time_diff)
gx_total += gx_delta
gy_total += gy_delta
rot_x = x_rotation(ax, ay, az)
rot_y = y_rotation(ax, ay, az)
# complementary Filter
last_x = K * (last_x + gx_delta) + (1 - K) * rot_x
# print(last_x)
PIDx = PID1.step(last_x)
pid1 = PIDx
# print pid1
# print targetvalue
PID1.setTarget(targetvalue)
if finished:
print kbdInput
finished = False
listener = threading.Thread(target=kbdListener)
listener.start()
if kbdInput == "t":
kbdInput = ""
targetvalue += 0.2
PID1.setTarget(targetvalue)
if kbdInput == "g":