def __init__(self,p_term=1,i_term=0.3,d_term=0.06,Dv_constant=0.4,ideal_angle=0.9):
self.pid_adjustment=PID(P=p_term,I=i_term,D=d_term)
self.sail=0
self.maxsail=math.pi/12*5
self.Dv_constant=Dv_constant
self.ideal_angle=ideal_angle
def __init__(self):
self.command_generator = PID(P=0.8, I=0.2, D=0.1)
self.rudder = 0
self.maxrudder = math.pi / 4 ##max angle of rudder
self.command_generator.setBoundary(self.maxrudder, -self.maxrudder)
self.sign_signal = 1
class rudder_controller():
def __init__(self):
self.command_generator = PID(P=0.8, I=0.2, D=0.1)
self.rudder = 0
self.maxrudder = math.pi / 4 ##max angle of rudder
self.command_generator.setBoundary(self.maxrudder, -self.maxrudder)
self.sign_signal = 1
def generate_command(self, desired_angle, current_angle, keeping_state,
velocity, tacking_angle, force_turning_angle,
boat_to_target_angle, true_wind):
if keeping_state != 2:
if math.cos(current_angle - desired_angle) > 0: ##防止坐标在-pi到pi时跳跃
if current_angle - desired_angle > math.pi / 2:
current_angle = current_angle - math.pi * 2
elif current_angle - desired_angle < -math.pi / 2:
current_angle = current_angle + math.pi * 2
else:
current_angle = current_angle
self.rudder = -self.command_generator.update(
current_angle, desired_angle)
current_angle = self.regular_angle(current_angle)
# print(self.rudder,'1')
else:
self.rudder = self.maxrudder * self.sign(
math.sin(current_angle - desired_angle))
if tacking_angle != None:
self.rudder == self.maxrudder * self.sign(
math.sin(tacking_angle - true_wind[1]))
# print(self.rudder,'2')
elif force_turning_angle != None:
if self.sign(self.rudder) == self.sign(
math.sin(force_turning_angle - true_wind[1])):
self.rudder = -self.rudder
# print(self.rudder,'3')
else:
self.rudder = self.maxrudder * self.sign(
math.sin(boat_to_target_angle - true_wind[1]))
if keeping_state == 4:
self.rudder = self.maxrudder * self.sign(self.rudder)
if velocity[0] < 0 and self.sign_signal > -0.8:
self.sign_signal -= 0.2
elif velocity[0] > 0 and self.sign_signal < 0.8:
self.sign_signal += 0.2
if self.sign_signal < 0:
self.rudder = -self.maxrudder * self.sign(
math.sin(desired_angle - true_wind[1])) / 2
# print(self.rudder)
return self.rudder
def regular_angle(self, angle):
while angle > math.pi:
angle -= math.pi * 2
while angle < -math.pi:
angle += math.pi * 2
return angle
def sign(self, p):
if p > 0:
return 1
elif p == 0:
return 0
else:
return -1
# a=rudder_controller()
# b=a.generate_command(0.6,0,0,[1,1,0,0])
# print(b)
# a=rudder_controller()
# b=a.generate_command(0.6,0,0,[1,1,0,0])
# print(b)
class sailcontroller():
def __init__(self,
p_term=1,
i_term=0.3,
d_term=0.06,
Dv_constant=0.4,
ideal_angle=0.9):
self.pid_adjustment = PID(P=p_term, I=i_term, D=d_term)
self.sail = 0
self.maxsail = math.pi / 12 * 5
self.Dv_constant = Dv_constant
self.ideal_angle = ideal_angle
def generate_command(self, velocity, position, target, target_v, true_wind,
keeping_state, desired_angle, tacking_angle,
force_turning_angle):
app_wind = self.get_app_wind(true_wind, position[3], velocity)
if target_v == 0.8:
self.pid_adjustment.clear()
if keeping_state == 0:
# print(3333333)
target_v = self.get_desire_v(velocity, position, target, true_wind,
keeping_state, desired_angle)
# print(target_v,11111111)
optimal_sail = self.get_optimal_sail(position[3], app_wind)
final_sail = self.get_final_sail(target_v, optimal_sail, velocity[0],
position[3], app_wind, tacking_angle)
if force_turning_angle != None:
final_sail = 1
return final_sail, target_v
def get_desire_v(self, velocity, position, target, true_wind,
keeping_state, desired_angle):
distance_st = math.sqrt(
pow(target[1] - position[1], 2) + pow(target[0] - position[0], 2))
if keeping_state == 0:
target_v = distance_st * self.Dv_constant
if math.cos(true_wind[1] - position[3]) > 0.3:
target_v = 0.2
# elif keeping_state==1:
# target_v=1.0
# # if math.cos(true_wind[1]-position[3])>math.cos(math.pi-self.ideal_angle):
# # target_v=0.15+0.01*math.asin(math.sin(abs(true_wind[1]-position[3])-(math.pi-self.ideal_angle)))
# # else:
# # target_v=0.15
# elif keeping_state==2:
# target_v=0.5
# elif keeping_state==3:
# target_v=0.2
# elif keeping_state==4:
# target_v=0.1
return target_v
def get_optimal_sail(self, heading_angle, app_wind):
# angle_diff=self.get_abs_angle_difference(heading_angle,app_wind[1]+math.pi)
# print('angle_diff',angle_diff,'heading',heading_angle,app_wind[1])
if math.cos(app_wind[1]) <= math.cos(3 * math.pi / 4):
optimal_sail = 0.3 + (-3 * math.pi / 4 + abs(app_wind[1])) * 0.3
# print(optimal_sail,'3')
else:
optimal_sail = 0.3 + (3 / 4 * math.pi - abs(app_wind[1])) * 1.2
# print(optimal_sail,'4')
if optimal_sail > self.maxsail:
optimal_sail = self.maxsail
return optimal_sail
def get_app_wind(self, true_wind, heading_angle, velocity):
###this part is different from the paper since there might be something wrong in the paper
###get coordinates of true wind
app_wind = [
true_wind[0] * math.cos(true_wind[1] - heading_angle) -
velocity[0],
true_wind[0] * math.sin(true_wind[1] - heading_angle) - velocity[1]
]
###convert into polar system
angle = math.atan2(app_wind[1], app_wind[0])
app_wind = [
math.sqrt(pow(app_wind[1], 2) + pow(app_wind[0], 2)), angle
]
return app_wind
def regular_angle(self, angle):
while angle > math.pi:
angle -= math.pi * 2
while angle < -math.pi:
angle += math.pi * 2
return angle
## return abs(angle1-angle2) which must < pi
def get_abs_angle_difference(self, angle1, angle2):
return math.acos(math.cos(angle1 - angle2))
def get_final_sail(self, target_v, optimal_sail, v, heading_angle,
app_wind, tacking_angle):
### get maxsail (considering the influence of wind)
maxsail = min(self.maxsail, abs(app_wind[1]))
offset = -self.pid_adjustment.update(v, target_v)
if self.maxsail - optimal_sail > 0.2:
final_sail = (maxsail + optimal_sail) / 2 + offset
# print('!!!!!!!!!',(maxsail+optimal_sail)/2)
if final_sail > maxsail:
final_sail = maxsail
elif final_sail < optimal_sail:
final_sail = optimal_sail
else:
final_sail = optimal_sail - 0.35 - offset
if final_sail > optimal_sail:
final_sail = optimal_sail
# print(optimal_sail)
elif final_sail < 0.4:
final_sail = 0.4
# print('offset',offset,'opt_sail',optimal_sail,'final sail',final_sail)
# print(self.pid_adjustment.PTerm,self.pid_adjustment.ITerm)
# print(optimal_sail,target_v,offset,final_sail,'22222222')
# if tacking_angle != None :
# # print('!!!!!!!!!!!!!!!!!!')
# final_sail=optimal_sail
return final_sail
class sailcontroller():
def __init__(self,
p_term=1,
i_term=0.3,
d_term=0.03,
Dv_constant=0.4,
ideal_angle=0.9):
self.pid_adjustment = PID(P=p_term, I=i_term, D=d_term)
self.sail = 0
self.maxsail = math.pi / 12 * 5
self.Dv_constant = Dv_constant
self.ideal_angle = ideal_angle
def generate_command(self, velocity, position, target, true_wind,
keeping_state, desired_angle, tacking_angle,
force_turning_angle):
app_wind = self.get_app_wind(true_wind, position[3], velocity)
target_v = self.get_desire_v(velocity, position, target, true_wind,
keeping_state, desired_angle)
optimal_sail = self.get_optimal_sail(position[3], app_wind)
final_sail = self.get_final_sail(target_v, optimal_sail, velocity[0],
position[3], app_wind, tacking_angle)
if force_turning_angle != None:
final_sail = 1
return final_sail, target_v
def get_desire_v(self, velocity, position, target, true_wind,
keeping_state, desired_angle):
distance_st = math.sqrt(
pow(target[1] - position[1], 2) + pow(target[0] - position[0], 2))
if keeping_state == 0:
target_v = distance_st * self.Dv_constant
if math.cos(true_wind[1] - position[3]) > 0.3:
target_v = 0.2
elif keeping_state == 1:
target_v = 0.45
elif keeping_state == 2:
target_v = 0.1
else:
target_v = 0
return target_v
def get_optimal_sail(self, heading_angle, app_wind):
# angle_diff=self.get_abs_angle_difference(heading_angle,app_wind[1]+math.pi)
if math.cos(app_wind[1]) <= math.cos(3 * math.pi / 4):
optimal_sail = 0.3 + (-3 * math.pi / 4 + abs(app_wind[1])) * 0.3
else:
optimal_sail = 0.3 + (3 / 4 * math.pi - abs(app_wind[1])) * 1.2
if optimal_sail > self.maxsail:
optimal_sail = self.maxsail
return optimal_sail
def get_app_wind(self, true_wind, heading_angle, velocity):
###this part is different from the paper since there might be something wrong in the paper
###get coordinates of true wind
app_wind = [
true_wind[0] * math.cos(true_wind[1] - heading_angle) -
velocity[0],
true_wind[0] * math.sin(true_wind[1] - heading_angle) - velocity[1]
]
###convert into polar system
angle = math.atan2(app_wind[1], app_wind[0])
app_wind = [
math.sqrt(pow(app_wind[1], 2) + pow(app_wind[0], 2)), angle
]
return app_wind
def regular_angle(self, angle):
while angle > math.pi:
angle -= math.pi * 2
while angle < -math.pi:
angle += math.pi * 2
return angle
def get_abs_angle_difference(self, angle1, angle2):
return math.acos(math.cos(angle1 - angle2))
def get_final_sail(self, target_v, optimal_sail, v, heading_angle,
app_wind, tacking_angle):
maxsail = min(self.maxsail, abs(app_wind[1]))
offset = -self.pid_adjustment.update(v, target_v)
if self.maxsail - optimal_sail > 0.2:
final_sail = (maxsail + optimal_sail) / 2 + offset
if final_sail > maxsail:
final_sail = maxsail
elif final_sail < optimal_sail:
final_sail = optimal_sail
else:
final_sail = optimal_sail - 0.35 - offset
if final_sail > optimal_sail:
final_sail = optimal_sail
elif final_sail < 0.4:
final_sail = 0.4
if tacking_angle != None:
final_sail = self.maxsail
return final_sail
