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
def thread(self, points, costfunc): self.animation_initialize(points) timer = PeriodTimer(0.1) i = 0 cost = [] while True: try: with timer: state = self.c.getNEData() cost.append(costfunc(state)) adata = [ state['x'], state['y'], state['u'], state['v'], state['phi'], state['alpha'], state['lm'], state['rm'] ] self.animate(adata, i, np.mean(cost)) i += 1 except: print('Error happened in drawing!!!!') break
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 b2a_little(bytes): # return array.array('d', bytes) # value=a2b_little(default_values) # print(b2a_little(value)[0]) # print(len(17.0)) from msgdev import MsgDevice,PeriodTimer from math import sin,cos if __name__=="__main__": dwa = MsgDevice() dwa.open() dwa.pub_bing('tcp://0.0.0.0:55010') interval = 0.3 t = PeriodTimer(interval) t.start() try: while True: with t: except KeyboardInterrupt: print('close') dwa.dev.close()
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')
# self.capture() # self.parse() self.publish() def close(self): self.s.close() print('VLP-16 Disconnected.') if __name__ == "__main__": try: dev = MsgDevice() dev.open() devinitial(dev) vlp = VLP(dev) t = PeriodTimer(t_refresh) t.start() while True: with t: print(time.strftime('%Y-%m-%d-%H-%M-%S', time.localtime())) vlp.update() except KeyboardInterrupt: vlp.close() dev.close() except Exception: vlp.close() dev.close() raise else: vlp.close()
ret["target"] = target # return ret return pickle.dumps(ret) def build_push_detection_dev(push_detection_dev): push_detection_dev.open() # push_detection_dev.pub_bind('tcp://0.0.0.0:55010') # 上传端口 push_detection_dev.sub_connect("tcp://192.168.0.8:55019") push_detection_dev.sub_add_url("det.data", [-100] * 7) if __name__ == "__main__": push_detection_dev = MsgDevice() build_push_detection_dev(push_detection_dev) from msgdev import PeriodTimer t = PeriodTimer(0.1) try: while True: with t: data = push_detection_dev.sub_get("det.data") print(data) # print(data, pickle.loads(data)) finally: push_detection_dev.pub_set("data", pickle.dumps(None)) push_detection_dev.close() logging.info("Everything Closed!")
except ValueError: self.logger.warning('invalid novatel cksum: ' + ps[1]) return False if cksum != novatel_crc32(self.header + ';' + self.content): self.logger.warning('invalid novatel cksum: ' + ps[1]) return False return True if __name__ == "__main__": gps_url = URL_BaseStation try: gps = GNSS(gps_url) gps.write(INIT_COMMANDS) t = PeriodTimer(0.2) t.start() aver_x = 0 aver_y = 0 aver_z = 0 n = 1 while n < 300: with t: gps.update() if gps.posx != 0: aver_x = 1.0 * (n - 1) / n * aver_x + 1.0 / n * gps.posx print(aver_x) aver_y = 1.0 * (n - 1) / n * aver_y + 1.0 / n * gps.posy aver_z = 1.0 * (n - 1) / n * aver_z + 1.0 / n * gps.posz n += 1 print('Fixed Position is:', aver_x, aver_y, aver_z)
from PID import PID from communicater import Communicater from decisionMaker import Maker # from drawer import Drawer from msgdev import PeriodTimer kp = 800 ki = 3 kd = 10 points = [[-42, 46], [-17, -34], [-85, -24], [-85, 46], [-42, 46]] if __name__ == '__main__': c = Communicater() pid = PID(kp=kp, ki=ki, kd=kd, minout=-2500, maxout=500, sampleTime=0.1) timer = PeriodTimer(0.1) maker = Maker(points) cost = 0 data = [] i = 0 while True: try: with timer: state = c.getNEData() ideal_angle = maker.getDecision(state) cost += maker.getCost(state) if ideal_angle == -1000: c.upload(0, 0) print('试验成功!')
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(): USE_PZH = True # initialize interface001, interface002, pzhdata = ship_initialize(True, True, True) t = PeriodTimer(0.2) diff_x_average_gps = MovingAverage(100) diff_y_average_gps = MovingAverage(100) diff_x_average_lidar = MovingAverage(100) diff_y_average_lidar = MovingAverage(100) # t.start() cnt = 0 end = 200 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') 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: print("No Target Specified!") continue else: cnt += 1 # print("Current CNT") goal = lidar_data[target] # goal = [x, y] diff_x = target_state[POS_X] - self_state[POS_X] diff_y = target_state[POS_Y] - self_state[POS_Y] diff_x_average_gps.update(diff_x) diff_y_average_gps.update(diff_y) diff_x_average_lidar.update(goal[0]) diff_y_average_lidar.update(goal[1]) phi2 = -atan2(diff_y_average_gps.avg, diff_x_average_gps.avg) - pi / 2 phi1 = atan2(diff_y_average_lidar.avg, diff_x_average_lidar.avg) out = phi1 + phi2 - pi / 2 # offset = atan2(diff_y_average_lidar.avg, diff_x_average_lidar.avg) - \ # atan2(diff_y_average_gps.avg, diff_x_average_gps.avg) print("[CNT {}] Current GPS ({}, {}), LiDAR ({}, {}). \ ph1{}, ph2 {}, out {} ({} deg).".format( cnt, diff_x_average_gps.avg, diff_y_average_gps.avg, diff_x_average_lidar.avg, diff_y_average_lidar.avg, phi1, phi2, out, out * 180 / pi)) if cnt >= end: break finally: import pickle import time def get_formatted_time(timestamp=None): if not timestamp: return time.strftime('%Y-%m-%d_%H-%M-%S', time.localtime()) else: return time.strftime('%Y-%m-%d_%H-%M-%S', time.localtime(timestamp)) if diff_y_average_lidar.avg is not None: phi2 = -atan2(diff_y_average_gps.avg, diff_x_average_gps.avg) - pi / 2 phi1 = atan2(diff_y_average_lidar.avg, diff_x_average_lidar.avg) out = phi1 + phi2 - pi / 2 out = atan2(diff_y_average_lidar.avg, diff_x_average_lidar.avg) - \ atan2(diff_y_average_gps.avg, diff_x_average_gps.avg) pickle.dump( { "offset": out, "timestamp": time.time(), "time": get_formatted_time() }, open("offset.pkl", "wb")) print("Data have saved to offset.pkl") else: print("Data is not received.") time.sleep(0.5) interface001.dev.close() interface002.dev.close() pzhdata.dev.close() print('dev 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')