def PID_Turn_old(pShip, pAim, yawOpt, yawShip, gz): # 原地转弯PID
# 错误！缺少return 0
global gz_last # 上一时刻角速度
PID_P_gz = 5
# PID_D_gz 在下面if结构中定义
yawAim = utils.getAimYaw(pShip, pAim) # 期望方向角
error = utils.angleDiff(yawAim, yawShip) # 角度差
gzAim = 10 * utils.symbol(error) # 期望角速度
if abs(error) > 100:
dire = 100 * utils.symbol(error) # 角度差大于100度时全速转弯
elif abs(error) < 20:
gzAim = gzAim * abs(error) / 20 * 0.5
error_gz = gzAim - gz # 角速度误差
dire = PID_P_gz * error_gz
if dire < 0:
dire = min(dire, -10)
else:
dire = max(dire, 10)
else:
error_gz = gzAim - gz # 角速度误差
d_gz = gz - gz_last # 角速度增量
gz_last = gz
if error_gz * d_gz > 0:
PID_D_gz = -1
else:
PID_D_gz = 1
dire = PID_P_gz * error_gz + 10 * utils.symbol(error)
gz_last = gz
dire = min(dire, 60)
dire = max(dire, -60)
return dire
def PID_Go(pShip, pAim, yawShip, gz): # 直行段PID
global thrust_last # 上一时刻油门值
PID_P = 0.5
# PID_D = 0 在下面if结构中定义
yawAim = utils.getAimYaw(pShip, pAim) # 期望方向角
error = utils.angleDiff(yawAim, yawShip) # 方向角误差
dist = utils.getDistance(pShip, pAim) # 距离误差
if error * gz > 0:
PID_D = -0.8
else:
PID_D = 0.8
dire = PID_P * error + gz * PID_D
dire = max(dire, -100)
dire = min(dire, 100)
thrust = 100 - abs(error) * 3
if dist < 10: # 接近段
tar_speed = dist / 8 - abs(error) / 20 # 目标速度
tar_speed = max(tar_speed, 0.2) # 目标速度限幅在0.2到1之间
tar_speed = min(tar_speed, 1)
err_speed = tar_speed - speed
thrust = thrust_last + err_speed * 50
thrust = abs(thrust)
thrust = max(thrust, 10) # 下限是10
thrust = min(thrust, 100) # 上限100
thrust_last = thrust
return int(dire), thrust
def PID_Go(pShip,
pAim,
yawShip,
gz,
yawOpt,
slowSpeed=True,
isFirst=True): # 直行段PID
global thrust_last # 上一时刻油门值
PID_P = 0.5
# PID_D = 0 在下面if结构中定义
if isFirst == True:
yawAim = utils.getAimYaw(pShip, pAim) # 期望方向角
else:
yawAim = yawOpt
# *********************补偿项***************注意，距离小的时候不准
# if utils.getAimYaw(pShip, pAim)>5:
# weight=np.array([0.7,0.3])
# yawAim=angleWeightSum(np.array(yawOpt,utils.getAimYaw(pShip, pAim)),weight)
# else:
# yawAim=yawOpt
error = utils.angleDiff(yawAim, yawShip) # 方向角误差
dist = utils.getDistance(pShip, pAim) # 距离误差
if error * gz > 0:
PID_D = -0.8
else:
PID_D = 0.8
dire = PID_P * error + gz * PID_D
dire = max(dire, -100)
dire = min(dire, 100)
thrust = (100 - abs(error) * 3) * 0.8
tar_speed = 0
if dist < 0.8 and slowSpeed == True:
tar_speed = dist / 1.5 - abs(error) / 20 # 目标速度
tar_speed = max(tar_speed, 0.2) # 目标速度限幅在0.2到1之间
tar_speed = min(tar_speed, 1)
err_speed = tar_speed - speed
thrust = thrust_last + err_speed * 50
thrust = abs(thrust)
thrust = max(thrust, 10) # 下限是10
thrust = min(thrust, 100) # 上限100
thrust_last = thrust
log.debug("PID_Go 目标速度：{}\t实际速度: {}\tthrust: {}\tdist: {}".format(
tar_speed, speed, thrust, dist))
return int(dire), thrust
def PID_Turn(pShip,
pAim,
yawShip,
gz,
yawOpt,
isFirst=True,
error=None,
isLeft=None,
routeType=0): # 原地转弯PID
PID_P_gz = 5
# PID_D_gz 在下面if结构中定义
if error == None:
if isFirst == True:
yawAim = utils.getAimYaw(pShip, pAim) # 期望方向角
else:
yawAim = yawOpt
error = utils.angleDiff(yawAim, yawShip) # 角度差
# if isLeft != None and abs(error) > 120:
# if (isLeft and error > 0):
# error -= 360
# elif not isLeft and error < 0:
# error += 360
if routeType == 0 or routeType == -1:
# 循迹
gzAim = 15 * utils.symbol(error) # 期望角速度
if abs(error) > 100:
dire = 100 * utils.symbol(error) # 角度差大于100度时全速转弯
elif abs(error) < 20:
gzAim = gzAim * abs(error) / 20 / 2
error_gz = gzAim - gz # 角速度误差
dire = PID_P_gz * error_gz
if dire < 0:
dire = min(dire, -20)
else:
dire = max(dire, 20)
if abs(error) < 5:
return 0
else:
error_gz = gzAim - gz # 角速度误差
dire = PID_P_gz * error_gz + 10 * utils.symbol(error)
# elif routeType == 1:
# # 区域
# gzAim = 20*utils.symbol(error) # 期望角速度
# if abs(error) > 60:
# dire = 100*utils.symbol(error) # 角度差大于100度时全速转弯
# elif abs(error) < 20:
# gzAim = gzAim*abs(error)/20/2
# error_gz = gzAim-gz # 角速度误差
# dire = PID_P_gz * error_gz
# if dire < 0:
# dire = min(dire, -30)
# else:
# dire = max(dire, 30)
# if abs(error) < 5:
# return 0
# else:
# error_gz = gzAim-gz # 角速度误差
# dire = PID_P_gz * error_gz+10*utils.symbol(error)
if routeType == -1:
dire = min(dire, 60)
dire = max(dire, -60)
else:
dire = min(dire, 100)
dire = max(dire, -100)
log.debug("PID_Turn 角度差：{}\tdire: {}".format(error, dire))
return int(dire)