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)