def motor_control(deflection_angle):
global angle
#count = counter()
#print count
angle = deflection_angle
# PID参数初始化，设置Kp,Ki，Kd及控制量
pid = PID.Incremental_PID(1.25, 0.0, 0.025)
pid.setPoint = 0.0
if abs(angle) <= 2.0:
angle = 0.0
# PID 计算
pid.PID_compute(angle)
output = pid.output
print('output', output)
if output >= 50:
output = 50
elif output < -50:
output = -50
#output = output*1.5
#output = output #(-30 30)
r_duty = int(mid + output)
l_duty = int(mid - output)
# 速度上下限设置，不同电机需调整
if r_duty < 50:
r_duty = 50
if l_duty < 50:
l_duty = 50
if r_duty >= 95:
r_duty = 95
if l_duty >= 95:
l_duty = 95
if r_duty - l_duty > 35:
r_duty = 95
l_duty = 0
elif l_duty - r_duty > 35:
r_duty = 0
l_duty = 95
print r_duty
print l_duty
# 电机PWM控制
Run.forward(r_duty, l_duty)
def motor_control(deflection_angle):
global time_list
global error_list
global angle
#count = counter()
#print count
angle = deflection_angle
pid = PID.Incremental_PID(1.25, 0.0, 0.025)
pid.setPoint = 0.0
if abs(angle) <= 3.0:
angle = 0.0
pid.PID_compute(angle)
output = pid.output
print('output', output)
if output > 50:
output = 50
elif output < -50:
output = -50
output = output * 1.5
output = output #(-30 30)
r_duty = int(mid + output)
l_duty = int(mid - output) + 2
#if abs(r_duty - l_duty) < 10:
# r_duty += 5
# l_duty += 5
if r_duty < 40:
r_duty = 40
if l_duty < 40:
l_duty = 40
if r_duty >= 80:
r_duty = 80
if l_duty >= 80:
l_duty = 80
if r_duty - l_duty > 20:
r_duty = 80
l_duty = 20
elif l_duty - r_duty > 20:
r_duty = 20
l_duty = 80
print r_duty
print l_duty
Run.r_forward(r_duty, l_duty)
def motor_control(point_x, point_y, mid_PWM):
global time_list
global error_list
global angle
if point_x > 0 and point_x < 640:
pid = PID.Incremental_PID(1.05, 0.0, 0.02)
pid.setPoint = 90.0
if point_x > Width / 2 + 5:
angle = 90 - math.degrees(
math.atan(float(point_x - 160) / float(240 - point_y)))
elif point_x < Width / 2 - 5:
angle = 90 + math.degrees(
math.atan(float(160 - point_x) / float(240 - point_y)))
else:
angle = 90.0
pid.PID_compute(angle)
output = pid.output
print('output', output)
if output > 90:
output = 90
elif output < -90:
output = -90
output = output / 1.2 #(-30 30)
if abs(output) < 1.0:
output = 0
# AI car motor2.0
if mid_PWM != 0:
r_duty = int(mid_PWM + output)
l_duty = int(mid_PWM - output)
if r_duty < 55:
r_duty = 55
if l_duty < 55:
l_duty = 55
if r_duty >= 100:
r_duty = 100
if l_duty >= 100:
l_duty = 100
if r_duty - l_duty > 30:
r_duty = 72
l_duty = 0
elif l_duty - r_duty > 30:
r_duty = 0
l_duty = 72
else:
if abs(output) < 3:
r_duty = 0
l_duty = 0
else:
if output > 8:
output = 8
if output < -8:
output = -8
r_duty = int(output * 10)
l_duty = int(-output * 10)
else:
r_duty = 0
l_duty = 0
print r_duty
print l_duty
Run.forward(r_duty, l_duty)
