def __init__(self,
velocity=[0, 0],
heading_angle=0,
course_angle=0,
desired_angle=0,
angular_velocity=0,
location=[2, 3],
rudder=0,
sail=0,
sample_time=0.1,
target=[3, 5],
IMU_sample_time=0.1,
data_base_sample_time=0.1):
#### all the units of angles are rad
self.velocity = velocity ###[v,u], where v is the heading angle of the sailboat
self.heading_angle = heading_angle
self.course_angle = course_angle
self.desired_angle = desired_angle
self.angular_velocity = angular_velocity ## w
self.location = location ###[x,y]
self.rudder = rudder ### the positive angle is corresponding to counterclockwise
self.sail = sail ### the positive angle is corresponding to counterclockwise
self.choosen_angle = 0
self.target = target ###the center of target area[x,y] self.dM=10,which is the radius of the pre-arrived area ,self.dT=5,which is r of target area
self.desired_acc = 0
self.frequency = 10
self.dT = 1 ## radius of target area
self.dM = 1.8 ## radius of pre arrive
self.desired_v = 0
self.maxrudder = math.pi / 4 ##max angle of rudder
self.maxsail = math.pi / 12 * 5
self.true_wind = [5, math.pi * 3 / 2] ##[wind speed, direction]
self.app_wind = [0, 0] ##[wind speed, direction]
self.sample_time = 0.01 ##time for every single update
###need to be adjusted ###
self.time = 0
self.rudder_controller = PID()
self.sail_speed_controller = PID(0.1, 0.01, 0.05)
self.d = 4
self.if_acc = 0
#this part is to add the boundary
self.x_value = 1
self.y_value = 1
self.flag = False
self.if_keeping = False
self.keeping_state = 1 ##1: before turning 2: turning 3:recover 4:go back
self.v_list = [0] * 7
self.u_list = [0] * 7
self.w_list = [0] * 7
def __init__(self,
velocity=[0, 0],
choosen_angle=0,
heading_angle=0,
course_angle=0,
desired_angle=0,
angular_velocity=0,
location=[0, 0],
rudder=0,
sail=0,
jibsail=0,
sample_time=0.1,
acc_v=0,
acc_u=0,
acc_w=0,
desired_acc=0):
#### all the units of angles are rad
self.velocity = velocity ###[v,u], where v is the heading angle of the sailboat
self.heading_angle = heading_angle
self.course_angle = course_angle
self.desired_angle = desired_angle
self.angular_velocity = angular_velocity ## w
self.location = location ###[x,y]
self.rudder = rudder ### the positive angle is corresponding to counterclockwise
self.sail = sail ### the positive angle is corresponding to counterclockwise
self.choosen_angle = choosen_angle
#self.jibsail=jibsail ### to be continued
self.acc_v = acc_v
self.acc_u = acc_u
self.acc_w = acc_w
self.target = [
3, 6
] ###the center of target area[x,y] self.dM=10,which is the radius of the pre-arrived area ,self.dT=5,which is r of target area
self.desired_acc = desired_acc
self.p1 = 0.01 ##drift coefficient####relative big for the plastic sailboat
self.p2 = 2.5 ##tangential friction
self.p3 = 0.8 ##angular friction
self.p4 = 1.5 ##sail lift
self.p5 = 30 ##rudder lift
self.p6 = 0.05 ##distance to sail
self.p7 = 0.05 ##distance to mast
self.p8 = 0.25 ##distance to rudder
self.p9 = 3 ##mass of boat
self.p10 = 0.3 ##moment of inertia
self.p11 = 0.3 ##rudder break coefficient
self.dT = 0.7
self.dM = 1.4
self.boat_size = 0.1 ##which equals that the length of the boat is 0.3m
self.maxrudder = math.pi / 4 ##max angle of rudder
self.maxsail = math.pi / 12 * 5
self.true_wind = [5, math.pi * 3 / 2] ##[wind speed, direction]
self.app_wind = [0, 0] ##[wind speed, direction]
self.sample_time = 0.01 ##time for every single update
self.ks = 50 ###need to be adjusted ###
self.aligned_p = 0.1 ####need to be adjusted ##about 15 degrees of the mini sailboat
self.q = -1 ##the parameter for tacking which is 1 or -1
self.time = 0
self.rudder_controller = PID()
self.n = 0.1
self.if_acc = 0
# self.if_tacking=0
#this part is to add the boundary
target_boat_angle = math.atan2(self.target[1] - self.location[1],
self.target[0] - self.location[0])
self.x_value = self.sign(math.cos(target_boat_angle))
self.y_value = self.sign(math.sin(target_boat_angle))
self.ser = serial.Serial('COM5', 57600)
self.b = 0
def __init__(self,
velocity=[0, 0],
heading_angle=0,
course_angle=0,
desired_angle=0,
angular_velocity=0,
location=[2, 3],
rudder=0,
sail=0,
sample_time=0.1,
target=[3, 6],
IMU_sample_time=0.1,
data_base_sample_time=0.1):
#### all the units of angles are rad
self.velocity = velocity ###[v,u], where v is the heading angle of the sailboat
self.heading_angle = heading_angle
self.next_desired_angle = 0
self.desired_angle = desired_angle
self.angular_velocity = angular_velocity ## w
self.location = location ###[x,y]
self.rudder = rudder ### the positive angle is corresponding to counterclockwise
self.sail = sail ### the positive angle is corresponding to counterclockwise
self.last_v = 0
self.roll = 0
self.roll_angular_velocity = 0
self.target = target ###the center of target area[x,y] self.dM=10,which is the radius of the pre-arrived area ,self.dT=5,which is r of target area
self.frequency = 10
self.dT = 1.4 ## radius of target area
self.dM = 2.8 ## radius of pre arrive
self.desired_v = 0
self.maxrudder = math.pi / 4 ##max angle of rudder
self.maxsail = math.pi / 12 * 5
self.true_wind = [3, math.pi * 3 / 2] ##[wind speed, direction]
self.app_wind = [3, math.pi * 3 / 2] ##[wind speed, direction]
###need to be adjusted ###
self.tacking_sign = 0
self.tacking_velocity = 0.5
self.find_sail_sign = 1
self.optimal_sail_adjustment = 0
self.p1 = 0 ##drift coefficient####relative big for the plastic sailboat
self.p2 = 4 ##tangential friction
self.p3 = 2.5 ##angular friction
self.p4 = 1.8 ##sail lift
self.p5 = 30 ##rudder lift
self.p6 = 0.05 ##distance to sail
self.p7 = 0.05 ##distance to mast
self.p8 = 0.25 ##distance to rudder
self.p9 = 3 ##mass of boat
self.p10 = 0.3 ##moment of inertia
self.p11 = 0.3
self.p12 = 0.002 #wind effect on w
self.p13 = 0.045 #wind effect on v
self.time = 0
self.rudder_controller = PID()
self.d = 3.65
#this part is to add the boundary
self.flag = False
self.if_keeping = False
self.keeping_state = 1 ##1: before turning 2: turning 3:recover 4:go back
self.start_tacking_time = -1
self.init_tacking_sign = 0
self.v_list = [0] * 5
self.u_list = [0] * 5
self.w_list = [0] * 5
self.p_list = [0] * 5
self.tacking_state = 'not tacking'
self.if_force_turning = False
self.tacking_angle = 0
self.go_to_center_start_time = 0
self.go_to_center_angle = 0.5
self.sleep_time = 0
def execute_next(self):
action = self.actions.pop(0)
direction = None
if action == "MoveF" or action == "MoveB":
current_pose = self.pose
quat = (current_pose.orientation.x, current_pose.orientation.y,
current_pose.orientation.z, current_pose.orientation.w)
euler = tf.transformations.euler_from_quaternion(quat)
current_yaw = euler[2]
if current_yaw > (-math.pi / 4.0) and current_yaw < (math.pi /
4.0):
print "Case 1"
#raw_input()
target_pose = copy.deepcopy(current_pose)
target_pose.position.x += 0.5
#direction = 'x'
#incr y co-ordinate
elif current_yaw > (math.pi /
4.0) and current_yaw < (3.0 * math.pi / 4.0):
print "Case 2"
#raw_input()
target_pose = copy.deepcopy(current_pose)
target_pose.position.y += 0.5
#direction = 'y'
#decr x co
elif current_yaw > (-3.0 * math.pi /
4.0) and current_yaw < (-math.pi / 4.0):
print "Case 3"
#raw_input()
target_pose = copy.deepcopy(current_pose)
target_pose.position.y -= 0.5
#direction = '-y'
else:
print "Case 4"
#raw_input()
target_pose = copy.deepcopy(current_pose)
target_pose.position.x -= 0.5
#direction = '-x'
PID(target_pose, "linear").publish_velocity()
elif action == "TurnCW" or action == "TurnCCW":
current_pose = self.pose
quat = (current_pose.orientation.x, current_pose.orientation.y,
current_pose.orientation.z, current_pose.orientation.w)
euler = tf.transformations.euler_from_quaternion(quat)
yaw = euler[2]
if action == "TurnCW":
target_yaw = yaw - (math.pi / 2.0)
if target_yaw < -math.pi:
target_yaw += (math.pi * 2)
else:
target_yaw = yaw + (math.pi / 2.0)
if target_yaw >= (math.pi):
target_yaw -= (math.pi * 2)
target_pose = Pose()
target_pose.position = current_pose.position
target_quat = Quaternion(*tf.transformations.quaternion_from_euler(
euler[0], euler[1], target_yaw))
target_pose.orientation = target_quat
print target_pose.orientation
PID(target_pose, "rotational").publish_velocity()
else:
print "Invalid action"
exit(-1)
if len(self.actions) == 0:
self.status_publisher.publish(self.free)