def main(): arduinoSocket = socket.socket(socket.AF_INET, socket.SOCK_STREAM) arduinoSocket.connect((host, port)) # arduinoSocket.setblocking(0) t = PeriodTimer(0.1) t.start() count = 0 num1 = 105 num2 = 50 while True: try: with t: count += 1 num1 += 1 num2 += 1 if num1 > 120: num1 = 100 if num2 > 130: num2 = 50 print count data = [num1, num2] arduinoSocket.send(dataSend(data)) print data print dataRead(arduinoSocket) except (Exception, KeyboardInterrupt): arduinoSocket.close()
def main(): mio = MIO_TRANS() t = PeriodTimer(T) t.start() try: while True: with t: mio.update() except (Exception, KeyboardInterrupt): mio.dev.close() mio.jsdev.close() mio.tmdev.close() if Serial_on: mio.Arduino_ser.close() raise
def main(): Arduino_ser = serial.Serial(port='COM3', baudrate=9600, bytesize=8, parity='N', stopbits=1, timeout=1) t = PeriodTimer(0.1) t.start() data = 0 while True: with t: data += 1 if data > 360: data = 0 Arduino_ser.write(send_data(data)) print data, data + 1, data + 2, data + 3 read_data(Arduino_ser)
def main(): arduinoSer = serial.serial_for_url(arduinoUrl, baudrate=115200, do_not_open=True, timeout=1) dev = MsgDevice() dev.open() dev.pub_bind(msgPubBind) dev.sub_connect(msgSubConnect) dev.sub_add_url('motorSpeed') dev.sub_add_url('maxPower') for i in range(7): dev.sub_add_url('rudder' + str(i) + 'Ang') t = PeriodTimer(0.1) t.start() try: try: arduinoSer.open() except serial.serialutil.SerialException, e: arduinoSer.close() print e, ", trying reconnet..." while True: with t: # data = subFromVeristand(dev) data = [140, 10, 130, 50, 50, 130, 50, 130, 50] print data if not arduinoSer._isOpen: try: arduinoSer.open() except serial.serialutil.SerialException, e: print e, ", trying reconnect..." try: if arduinoSer._isOpen: arduinoSer.write(dataSend(data)) dataFromArduino = dataRead(arduinoSer) print dataFromArduino except serial.serialutil.SerialException, e: arduinoSer.close() try: if dataFromArduino: pubToVeristand(dev, dataFromArduino) except UnboundLocalError: pass
def main(): Arduino_ser = serial.Serial(port='COM9', baudrate=9600, bytesize=8, parity='N', stopbits=1, timeout=1) t = PeriodTimer(0.1) t.start() data = 0 while True: with t: # data += 1 # if data > 254: # data = 0 data = [10, 130] Arduino_ser.write(send_data(data)) print data read_data(Arduino_ser)
def main(): # arduinoSocket = socket.socket(socket.AF_INET, socket.SOCK_STREAM) arduinoSer = serial.serial_for_url( arduinoUrl, baudrate=115200, do_not_open=True, timeout=1) dev = MsgDevice() dev.open() dev.pub_bind(msgPubBind) dev.sub_connect(msgSubConnect) dev.sub_add_url('motorSpeed') dev.sub_add_url('rudderAng') dev.sub_add_url('sailAng') t = PeriodTimer(0.1) t.start() try: try: arduinoSer.open() except serial.serialutil.SerialException, e: arduinoSer.close() print e, ", trying reconnet..." while True: with t: data = subFromVeristand(dev) # data = [100, 90, 0] print "SEND: ", data if not arduinoSer._isOpen: try: arduinoSer.open() except serial.serialutil.SerialException, e: print e, ", trying reconnect..." try: if arduinoSer._isOpen: arduinoSer.write(dataSend(data)) dataFromArduino = dataRead(arduinoSer) print "RECV: ", dataFromArduino except serial.serialutil.SerialException, e: arduinoSer.close() try: if dataFromArduino: pubToVeristand(dev, dataFromArduino) except UnboundLocalError: pass
from msgdev import MsgDevice, PeriodTimer import numpy as np if __name__=="__main__": dev=MsgDevice() dev.open() dev.pub_bind('tcp://0.0.0.0:55005') data = np.loadtxt('log-05-05-17-07.txt', skiprows=1, usecols=(3,4), delimiter=',') i = 0 t=PeriodTimer(0.1) t.start() try: while True: with t: dev.pub_set1('ahrs.yaw', data[i,0]) dev.pub_set1('ahrs.yaw_speed', data[i,1]) print('ahrs.yaw', data[i,0], 'ahrs.yaw_speed', data[i,1]) i += 1 if i == len(data): i = 0 except (KeyboardInterrupt, Exception) as e: print('closed') finally: pass
def main(args): USE_PZH = args.use_pzh PP = args.pp DWA = args.dwa # initialize interface001, interface002, pzhdata = ship_initialize(True, True, USE_PZH) pid = PID(kp=kp, ki=ki, kd=kd, minout=-1000, maxout=1000, sampleTime=0.1) u = np.array([0.0, 0.0]) # speed, yaw speed config = Config() # initial state [x(m), y(m), yaw(rad), v(m/s), omega(rad/s)] # 需要根据实际出发点赋值 x = np.array([0.0, 0.0, math.pi / 8.0, 0.0, 0.0]) # goal position [x(m), y(m)] goal = np.array([-90, 3]) # obstacles obstacles = [] csv_file = csv.writer( open( "log-{:s}.txt".format( time.strftime("%m-%d-%H-%M", time.localtime())), 'w')) csv_file.writerow([ "timestamp", 'gps.posx', 'gps.posy', 'ahrs.yaw', 'ahrs.yaw_speed', 'gps.hspeed', 'gps.stdx', 'gps.stdy', 'gps.track', 'target.posx', 'target.posy', 'target.yaw', 'target.yaw_speed', 'target.hspeed', 'target.stdx', 'target.stdy', 'target.track', 'distance', 'left_motor', 'right_motor' ]) t = PeriodTimer(0.5) t.start() try: while True: with t: self_state = interface001.receive('gps.posx', 'gps.posy', 'ahrs.yaw', 'ahrs.yaw_speed', 'gps.hspeed', 'gps.stdx', 'gps.stdy', 'gps.track') target_state = interface002.receive('gps.posx', 'gps.posy', 'ahrs.yaw', 'ahrs.yaw_speed', 'gps.hspeed', 'gps.stdx', 'gps.stdy', 'gps.track') obstacles.append([target_state[POS_X], target_state[POS_Y]]) # [num_obstacles, 2] # TODO 为什么把对手船也当作obstacle来看待? # TODO 而且如果你用我的数据,这个地方不用做处理吗? if USE_PZH: assert pzhdata is not None lidar_data = pzhdata.receive() if lidar_data["terminated"]: print( "Peng Zhenghao's program is terminated. For safety we close this program." ) break target = lidar_data["target"] if not target: # TODO 你的代码是完全不支持发呆状态的。所以如果target没有指定会发生什么事情? print("No Target Specified!") # continue TODO 也许可以用continue跳过后面的所有东西,但这样可能有意外后果。 else: goal = lidar_data[target] # goal = [x, y] for key, centroid in lidar_data.items(): if key.startswith( "Object") and centroid and key != target: obstacles.append(centroid) obstacles = trans_coord( [self_state[POS_X], self_state[POS_Y]], self_state[YAW], np.array(obstacles)) if PP: # TODO 所以如果你要用我的target坐标来做pure pursuit呢? target_angle, dist = pure_pursuit(self_state, target_state) output = pid.compute(self_state[YAW], target_angle) # TODO 你确定 yaw 和 target angle的零点与单位(rad or degree)一样? elif DWA: x = [ self_state[POS_X], self_state[POS_Y], self_state[YAW], self_state[SPD], self_state[YAW_SPEED] ] u, ltraj = dwa_control(x, u, config, goal, obstacles) ideal_angle = self_state[YAW] + u[1] * 0.1 output = pid.compute(self_state[YAW], ideal_angle) else: output = 0 ################## 此处往下没有问题 ####################### # dead band output = 0 if abs(output) < 5 else output # 本船距离目标的距离 dist = math.sqrt((self_state[POS_X] - goal[0])**2 + (self_state[POS_Y] - goal[1])**2) if dist <= 3: average = baseline[0] elif dist <= 10: average = baseline[1] else: average = baseline[2] left_motor, right_motor = (average + output / 2), average - output / 2 left_motor = -rpm_limit(left_motor) right_motor = rpm_limit(right_motor) print( 'self ship state: ', 'posx:{},posy:{},yaw:{},speed:{},left:{},right:{}'.format( self_state[POS_X], self_state[POS_Y], self_state[YAW], self_state[SPD], left_motor, right_motor)) print( 'target ship state: ', 'posx:{}, posy: {},distance:{}'.format( target_state[POS_X], target_state[POS_Y], dist)) interface001.Motor_send(left_motor, right_motor) csv_file.writerow([time.time()] + self_state + target_state + [dist, left_motor, right_motor]) finally: interface001.Motor_send(0, 0) time.sleep(0.1) interface001.dev.close() interface002.dev.close() csv_file.close() print('everything closed')
def main(): # USE_PZH = args.use_pzh # PP = args.pp # DWA = args.dwa # initialize interface001, interface002, pzhdata = ship_initialize(True, True, USE_PZH) pid = PID(kp=kp, ki=ki, kd=kd, minout=-1000, maxout=1000, sampleTime=0.1) u = np.array([0.0, 0.0]) # speed, yaw speed config = Config() # initial state [x(m), y(m), yaw(rad), v(m/s), omega(rad/s)] # 需要根据实际出发点赋值 x = np.array([0.0, 0.0, math.pi / 8.0, 0.0, 0.0]) # goal position [x(m), y(m)] goal = np.array([-90, 3]) predefiend_goal = goal.copy() # obstacles obstacles = [] csv_source = open("log-{:s}.txt".format(time.strftime("%m-%d-%H-%M", time.localtime())), 'w') csv_file = csv.writer(csv_source) csv_file.writerow( ["timestamp", 'gps.posx', 'gps.posy', 'ahrs.yaw', 'ahrs.yaw_speed', 'gps.hspeed', 'gps.stdx', 'gps.stdy', 'gps.track', 'target.posx', 'target.posy', 'target.yaw', 'target.yaw_speed', 'target.hspeed', 'target.stdx', 'target.stdy', 'target.track', 'distance', 'left_motor', 'right_motor']) # 计算频率 interval = 0.5 t = PeriodTimer(interval) t.start() try: while True: with t: self_state = interface001.receive('gps.posx', 'gps.posy', 'ahrs.yaw', 'ahrs.yaw_speed', 'gps.hspeed', 'gps.stdx', 'gps.stdy', 'gps.track') target_state = interface002.receive('gps.posx', 'gps.posy', 'ahrs.yaw', 'ahrs.yaw_speed', 'gps.hspeed', 'gps.stdx', 'gps.stdy', 'gps.track') # target_state = interface001.receive('gps.posx', 'gps.posy', 'ahrs.yaw', # 'ahrs.yaw_speed', 'gps.hspeed', # 'gps.stdx', 'gps.stdy', 'gps.track') # self_state = interface002.receive('gps.posx', 'gps.posy', 'ahrs.yaw', # 'ahrs.yaw_speed', 'gps.hspeed', # 'gps.stdx', 'gps.stdy', 'gps.track') if USE_PZH: assert pzhdata is not None lidar_data = pzhdata.receive() if lidar_data["terminated"]: print( "Peng Zhenghao's program is terminated. For safety we close this program.") break #### FOR DEBUG # print("Peng Zehgnhao data: ", lidar_data) # continue target = lidar_data["target"] if not target: # 如果没有搜索到target, 则用初始化的goal作为目标点 # TODO 所以如果target没有指定会发生什么事情? print("No Target Specified! Heading to predefiend_goal") goal = predefiend_goal else: goal = target # goal = [x, y] for key, centroid in lidar_data.items(): if key.startswith("Object") and centroid and key != target: obstacles.append(centroid) obstacles = trans_coord([self_state[POS_X], self_state[POS_Y]], self_state[YAW], np.array(obstacles)) assert obstacles.shape[1] == 2 obstacles = obstacles.tolist() else: # 如果不用PZH, 则使用避障目标船的GPS obstacles = [[target_state[POS_X], target_state[POS_Y]]] # [num_obstacles, 2] if PP: if USE_PZH: target_angle, dist = pure_pursuit(self_state, goal) else: target_angle, dist = pure_pursuit(self_state, target_state) output = pid.compute(self_state[YAW], target_angle) # yaw, target_angle unit: rad elif DWA: x = [self_state[POS_X], self_state[POS_Y], self_state[SPD_DIR], self_state[SPD], self_state[YAW_SPEED]] u, ltraj = dwa_control(x, u, config, goal, obstacles) ideal_angle = self_state[SPD_DIR] + u[1] * interval output = pid.compute(self_state[SPD_DIR], ideal_angle) * 50 # 输出太小??? print('output', output, 'self_state[SPD_DIR]', self_state[SPD_DIR], 'ideal_angle', ideal_angle) # 本船距离目标的距离 dist = math.sqrt((self_state[POS_X] - goal[0]) ** 2 + (self_state[POS_Y] - goal[1]) ** 2) else: output, dist = 0, 0 # dead band diff = 0 if abs(output) < 5 else output # 根据距离给速度分级 # TODO 改为速度控制器 if dist <= 3: average = baseline[0] elif dist <= 10: average = baseline[1] else: average = baseline[2] left_motor, right_motor = (average + diff / 2), (average - diff / 2) # 除以2, 转弯时超调小 left_motor = -rpm_limit(left_motor) right_motor = rpm_limit(right_motor) print('self ship state: ', 'posx:{},posy:{},yaw:{},speed:{},left:{},right:{}'.format(self_state[POS_X], self_state[POS_Y], self_state[SPD_DIR], self_state[SPD], left_motor, right_motor)) print('target ship state: ', 'posx:{}, posy: {},distance:{}'.format(target_state[POS_X], target_state[POS_Y], dist)) print('goal', goal, 'obstacles', obstacles) interface001.Motor_send(left_motor, right_motor) csv_file.writerow( [time.time()] + self_state + target_state + [dist, left_motor, right_motor]) finally: interface001.Motor_send(0, 0) time.sleep(interval) interface001.dev.close() interface002.dev.close() pzhdata.dev.close() csv_source.close() print('everything closed')
def main(): # auv = auv_initialize() auv = MsgDevice() auv.open() auv.pub_bind('tcp://0.0.0.0:55004') # Pure Pursuit initialize target_angle = 0 dist = 0 # DWA initialize configDWA = ConfigDWA() dynamic_window = DWA(configDWA) # initial state [x(m), y(m), yaw(rad), speed(m/s), yaw_speed(rad/s)] # init_state = np.array([0, 10, 45 * pi / 180, 0.0, 0.0]) init_state = np.array([0, 0, 0, 0.0, 0.0]) # goal position [x(m), y(m)] predefined_goal = np.array([90.0, 80.0]) goal = predefined_goal # obstacles [x(m) y(m), spd(m/s), yaw] # static_obstacles = np.array([[100.0, 70.0, 0.0, 0.0], # [30.0, 39.0, 0, 0.0], # [30.0, 70.0, 0.0, 0.0], # [50.0, 70.0, 0.0, 0.0], # [80, 70, 0, 0], # [60, 40, 0, 0], # [40, 40, 0, 0], # [70, 80, 0, 0], # [20.0, 20.0, 0.0, 0.0], # ]) static_obstacles = np.array([ [90.0, 60.0, 0.0, 0.0], [60.0, 70.0, 0, 0.0], [80.0, 60.0, 0.0, 0.0], [50.0, 70.0, 0.0, 0.0], [80, 70, 0, 0], [60, 40, 0, 0], [40, 50, 0, 0], [30, 60, 0, 0], [20.0, 30.0, 0.0, 0.0], ]) moving_obstacles = np.array([[20, 10, 0.7, 2], [60, 30, 0.2, 2]]) u = np.array([0.0, 0.0]) # RRT* initialize rrt_star = RRTStar(start=[init_state[0], init_state[1]], goal=goal, rand_area=[0, 100], obstacle_list=static_obstacles, expand_dis=5.0, path_resolution=1.0, goal_sample_rate=10, max_iter=100, connect_circle_dist=50.0) # path = rrt_star.planning(animation=True, search_until_max_iter=True) # path_index = -1 # localgoal = path[path_index] # time.sleep(3) traj = [init_state] best_traj = None state = init_state interval = 0.1 # Simulation time step pid_spd = PID(kp=2500.0, ki=100.0, kd=0, minout=-1000, maxout=1000, sampleTime=interval) pid_yawspd = PID(kp=15000, ki=500.0, kd=10, minout=-1500, maxout=1500, sampleTime=interval) pid_yaw = PID_angle(kp=800, ki=3, kd=10, minout=-1500, maxout=1500, sampleTime=interval) t = PeriodTimer(interval) t.start() i = 0 try: while True: with t: start_time = time.perf_counter() if i < 10000: i += 1 else: i = 0 goal = predefined_goal obstacle = np.vstack((moving_obstacles, static_obstacles)) min_dist = 100**2 for ob_single in moving_obstacles: dist = (state[0] - ob_single[0])**2 + (state[1] - ob_single[1])**2 if dist < min_dist: min_dist = dist # to_localgoal_dist_square = (state[POSX] - localgoal[0]) ** 2 + (state[POSY] - localgoal[1]) ** 2 # if to_localgoal_dist_square <= configDWA.robot_radius_square: # path_index -= 1 # if abs(path_index) < len(path): # localgoal = path[path_index] # else: # localgoal = goal # if min_dist > 8 ** 2: if 0: best_traj = None target_angle, dist = pure_pursuit(state, localgoal) if target_angle > pi: target_angle -= 2 * pi elif target_angle < -pi: target_angle += 2 * pi output = pid_yaw.compute( state[2], target_angle) # yaw, target_angle unit: rad # dead band diff = 0 if abs(output) < 5 else output # use different speed based on distance if dist <= 3: average = 800 elif dist <= 10: average = 1000 else: average = 1200 print('Pure Pursuit, Current path point: ', -path_index, "/", len(path)) else: u, best_traj = dynamic_window.update( state, u, goal, obstacle) u = [1, 0.1] # Forward velocity average = pid_spd.compute(state[3], u[0]) average = 0 if abs(average) < 5 else average # Yaw angle velocity diff = pid_yawspd.compute(state[4], u[1]) diff = 0 if abs(diff) < 5 else diff print('Dynamic Window Approach') # which path point to follow when switch to the pure pursuit # path_index = get_nearest_path_index(path, state)-len(path) if min_dist < 3**2: print('Collision!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!') print( info_format.format(i=i, real_spd=state[3], target_spd=u[0], real_yaw=state[2], target_yaw=target_angle, real_yawspd=state[4], target_yawspd=u[1], average_output=average, output_diff=diff, calc_time=time.perf_counter() - start_time)) if show_animation: traj.append(state.copy()) # plot(best_traj, state, goal, obstacle, path, traj) plot2(best_traj, state, goal, obstacle, traj) to_goal_dist_square = (state[POSX] - goal[0])**2 + (state[POSY] - goal[1])**2 if to_goal_dist_square <= configDWA.robot_radius_square: time.sleep(3) print("Goal!!") break # Simulation # state = trimaran_model(state, left, right, interval) state = AUV_model(state, average, -diff, interval) state = apply_noise(state) moving_obstacles = update_obstacle(moving_obstacles, interval) auv.pub_set1('posx', state[0]) auv.pub_set1('posy', state[1]) auv.pub_set1('speed', state[3]) auv.pub_set1('yaw', state[2]) auv.pub_set1('yawspd', state[4]) auv.pub_set1('speed_tar', u[0]) auv.pub_set1('yaw_tar', target_angle) auv.pub_set1('yawspd_tar', u[1]) finally: time.sleep(interval) print('everything closed')