def main(): logger = console_logger() dev = MsgDevice() gnss = GNSS(GNSS_URL, BAUDRATE, TIMEOUT, INIT_COMMANDS, logger) gnss.set_origin(ORIGIN_LAT, ORIGIN_LON) attrs = ( # weektime in seconds "time", # position x/y/z in m "posx", "posy", "posz", # standard deviation x/y/z in m "stdx", "stdy", "stdz", # num of satellites "satn", # horizontal speed (m/s), horizontal speed direction (rad, north is 0), vertical speed (m/s) "hspeed", "track", "vspeed") try: dev.open() for ep in MSG_PUB_BINDS: dev.pub_bind(ep) for ep in MSG_PUB_CONNS: dev.pub_connect(ep) while 1: gnss.update() for attr in attrs: dev.pub_set1('gps.' + attr, getattr(gnss, attr, 0)) except KeyboardInterrupt: pass finally: gnss.close() dev.close()
class MIO_TRANS(): def __init__(self): self.dev_connect_flag = 0 self.datashow_count = 0 self.logger = console_logger('MIO-5251') self.dev_init() self.ser_init() self.Servo_init() self.fst = struct.Struct('!4H') def dev_init(self): self.jsdev = MsgDevice() self.dev = MsgDevice() self.tmdev = MsgDevice() self.jsdev.open() self.dev.open() self.tmdev.open() try: self.tmdev.sub_connect('tcp://127.0.0.1:55006') self.logger.info('tmdev sub connect tcp://127.0.0.1:55006') if Local_test is True: self.dev.sub_connect(MSG_SUB_CONNECTS[0]) self.logger.info('dev sub connect ' + MSG_SUB_CONNECTS[0]) self.dev.pub_bind(MSG_PUB_BINDS) self.logger.info('dev pub bind ' + MSG_PUB_BINDS) self.jsdev.sub_connect(MSG_SUB_CONNECTS[2]) self.logger.info('jsdev sub connect ' + MSG_SUB_CONNECTS[2]) else: self.dev.sub_connect(MSG_SUB_CONNECTS[1]) self.logger.info('dev sub connect ' + MSG_SUB_CONNECTS[1]) self.dev.pub_bind(MSG_PUB_BINDS) self.logger.info('dev pub bind ' + MSG_PUB_BINDS) self.jsdev.sub_connect(MSG_SUB_CONNECTS[3]) self.logger.info('jsdev sub connect ' + MSG_SUB_CONNECTS[3]) self.jsdev.sub_add_url('js.Autoctrl') self.tmdev.sub_add_url('Transmitter_flag') except (zmq.error.ZMQError): self.dev.close() self.jsdev.close() self.logger.info('Address already in use') raise def ser_init(self): if not Serial_on: return try: self.Arduino_ser = serial.Serial(Arduino_port_addr, baudrate=9600, timeout=1) self.logger.info(self.Arduino_ser.portstr + ' open successfully!') self.Arduino_ser.flushInput() self.Arduino_read = Arduino_READ(self.Arduino_ser, self.dev) except (serial.serialutil.SerialException, OSError): self.dev.close() self.jsdev.close() self.logger.info('could not open port: ' + Arduino_port_addr) raise def Servo_init(self): self.motor = Servo('Motor_speed', 30, 0, self.dev, self.jsdev, self.tmdev) self.rudder = Servo('Rudder_ang', 40, 4, self.dev, self.jsdev, self.tmdev) self.mainsail = Servo('Mainsail_ang', 80, 0, self.dev, self.jsdev, self.tmdev) self.foresail = Servo('Foresail_ang', 70, 0, self.dev, self.jsdev, self.tmdev) def update(self): self.MIO_pub_sub() self.Arduino_update() if not Data_show: return self.DataInfoShow() def MIO_pub_sub(self): self.js_Autoctrl = int(self.jsdev.sub_get1('js.Autoctrl')) self.dev.pub_set1('js.Autoctrl', self.js_Autoctrl) self.dev_connect_flag = not self.dev_connect_flag self.dev.pub_set1('dev_connect_flag', self.dev_connect_flag) self.Transmitter_flag = self.tmdev.sub_get1('Transmitter_flag') self.motor.update(self.js_Autoctrl, self.Transmitter_flag) self.rudder.update(self.js_Autoctrl, self.Transmitter_flag) self.mainsail.update(self.js_Autoctrl, self.Transmitter_flag) self.foresail.update(self.js_Autoctrl, self.Transmitter_flag) global Data_show if self.Transmitter_flag == 1: Data_show = False def Arduino_update(self): if not Serial_on: return self.Arduino_read.update() self.Arduino_write() def Arduino_write(self): header = '\xff\x01' tmp = self.fst.pack(self.motor.c_data, self.rudder.c_data, self.mainsail.c_data, self.foresail.c_data) crc_code = struct.pack('!H', crc16(tmp)) tmp = header + tmp tmp = tmp + crc_code self.Arduino_ser.write(tmp) def DataInfoShow(self): self.datashow_count += 1 if self.datashow_count < 15: return self.datashow_count = 0 self.logger.info('''MIO received data from bank: Autoctrl, Motor, Rudder, Mainsail, Foresail: ''' + str([ self.js_Autoctrl, self.motor.r_data, self.rudder.r_data, self.mainsail.r_data, self.foresail.r_data ])) if not Serial_on: return self.logger.info('''Arduino to MIO data:) dogvane, mainsail, voltage, arduino_receive_flag: ''' + str([ self.Arduino_read.dogvane, self.Arduino_read.mainsail, self.Arduino_read.voltage, self.Arduino_read.arduino_receive_flag ])) self.logger.info('''MIO to Arduino data: ) motor_pwm, rudder_pwm, mainsail_pwm, foresail_pwm: ''' + str([ self.motor.c_data, self.rudder.c_data, self.mainsail.c_data, self.foresail.c_data ])) print
class Communicater(object): def __init__(self, upload=True): self.dev = MsgDevice() self.dev.open() self.dev.sub_connect('tcp://192.168.1.150:55003') # 下载端口 self.upload_flag = upload if upload: self.dev.pub_bind('tcp://0.0.0.0:55010') # 上传端口 self.dev.sub_add_url('') self.dev_gnss = MsgDevice() self.dev_gnss.open() self.dev_gnss.sub_connect('tcp://192.168.1.150:55004') self.dev_ahrs = MsgDevice() self.dev_ahrs.open() self.dev_ahrs.sub_connect('tcp://192.168.1.150:55005') self.dev_volt = MsgDevice() self.dev_volt.open() self.dev_volt.sub_connect('tcp://192.168.1.150:55006') self.data_down = [] self.data_up = [None, None] self.file_name = './data/' + time.strftime("%Y-%m-%d__%H:%M", time.localtime()) self.f = open(self.file_name + '.csv', 'w') self.writer = csv.writer(self.f) self.dev_gnss.sub_add_url('gps.posx'), self.dev_gnss.sub_add_url('gps.posy'), self.dev_gnss.sub_add_url('gps.posz'), self.dev_gnss.sub_add_url('gps.hspeed'), self.dev_gnss.sub_add_url('gps.vspeed'), self.dev_gnss.sub_add_url('gps.track') self.dev_ahrs.sub_add_url('ahrs.yaw'), self.dev_ahrs.sub_add_url('ahrs.yaw_speed'), self.dev_ahrs.sub_add_url('ahrs.acce_x'), self.dev_ahrs.sub_add_url('ahrs.acce_y'), self.dev_ahrs.sub_add_url('ahrs.acce_z'), self.dev.sub_add_url('left.Motor_SpeedCalc') self.dev.sub_add_url('right.Motor_SpeedCalc') self.dev_volt.sub_add_url('voltage') def download(self): gps = [ self.dev_gnss.sub_get1('gps.posx'), self.dev_gnss.sub_get1('gps.posy'), self.dev_gnss.sub_get1('gps.posz'), self.dev_gnss.sub_get1('gps.hspeed'), self.dev_gnss.sub_get1('gps.vspeed'), self.dev_gnss.sub_get1('gps.track') ] ahrs = [ self.dev_ahrs.sub_get1('ahrs.yaw'), self.dev_ahrs.sub_get1('ahrs.yaw_speed'), self.dev_ahrs.sub_get1('ahrs.acce_x'), self.dev_ahrs.sub_get1('ahrs.acce_y'), self.dev_ahrs.sub_get1('ahrs.acce_z'), ] l_m = self.dev.sub_get1('left.Motor_SpeedCalc') r_m = self.dev.sub_get1('right.Motor_SpeedCalc') volt = self.dev_volt.sub_get1('voltage') return gps, ahrs, l_m, r_m, volt def getNEData(self): """ :return: x,y,u,v,yaw,omega in NE sys. """ gps, ahrs, l_m, r_m, volt = self.download() yaw = ahrs[0] - 5 * math.pi / 180 U = gps[3] * math.cos(gps[5]) # 北东系速度 V = gps[3] * math.sin(gps[5]) self.data_down = [gps[0], gps[1], U, V, yaw, ahrs[1]] return { 'x': gps[0], 'y': gps[1], 'u': U, 'v': V, 'phi': yaw, 'alpha': ahrs[1], 'lm': l_m, 'rm': r_m } def upload(self, left, right): if self.upload_flag: self.dev.pub_set1('pro.left.speed', left) self.dev.pub_set1('pro.right.speed', right) self.data_up = [left, right] print('uploaded', left, right) else: self.data_up = [-1, -1] print('upload disabled') def record(self): self.writer.writerow(self.data_down + self.data_up) print('Saved: ', self.data_down + self.data_up) def __del__(self): self.upload(0, 0) self.dev.close() self.f.close()
aver = 0 state = joystick.get_axis(2) #processed data averspeed = aver * speed_limit * (-1) diffspeed = diff * speed_limit * 0.5 left_motor = (averspeed + diffspeed) * (-1) right_motor = averspeed - diffspeed if left_motor > speed_limit: left_motor = speed_limit if left_motor < -speed_limit: left_motor = -speed_limit if right_motor > speed_limit: right_motor = speed_limit if right_motor < -speed_limit: right_motor = -speed_limit if state >= 0: autoctrl = 1 if state < 0: autoctrl = 0 dev.pub_set1('js.left.speed', left_motor) dev.pub_set1('js.right.speed', right_motor) dev.pub_set1('js.autoctrl', autoctrl) if Debug == True: print("left motor speed is", left_motor) print("right motor speed is", right_motor) print("auto-control state is", autoctrl) except (Exception, KeyboardInterrupt, AbortProgram) as e: pygame.quit() dev.close()
self.dev.pub_set1(self.prefix+'.roll',self.roll) self.dev.pub_set1(self.prefix+'.pitch',self.pitch) self.dev.pub_set1(self.prefix+'.yaw',self.yaw) self.dev.pub_set1(self.prefix+'.roll_speed',self.roll_speed) self.dev.pub_set1(self.prefix+'.pitch_speed',self.pitch_speed) self.dev.pub_set1(self.prefix+'.yaw_speed',self.yaw_speed) self.dev.pub_set1(self.prefix+'.acce_x',self.acce_x) self.dev.pub_set1(self.prefix+'.acce_y',self.acce_y) self.dev.pub_set1(self.prefix+'.acce_z',self.acce_z) self.dev.pub_set1(self.prefix+'.devicestatus',self.devicestatus) #AHRS_URL = 'COM3' AHRS_URL = '/dev/serial/by-id/usb-Silicon_Labs_SBG_Systems_-_UsbToUart_001000929-if00-port0' if __name__ == "__main__": try: dev_pro = MsgDevice() dev_pro.open() dev_pro.pub_bind('tcp://0.0.0.0:55005') ahrs = AHRS(AHRS_URL,dev_pro) while True: ahrs.update() except (KeyboardInterrupt, Exception) as e: dev_pro.close() ahrs.close() raise finally: pass
t.start() while True: with t: autoctrl = dev_joy.sub_get1('js.autoctrl') dev_pro.pub_set1('autoctrl', autoctrl) print('Autoctrl:', autoctrl) left_motor.update(autoctrl) right_motor.update(autoctrl) print('rpm1', left_motor.Motor_SpeedCalc, 'rpm2', right_motor.Motor_SpeedCalc) #print LeftMotor.Motor_PWM, LeftMotor.Motor_Current, LeftMotor.Motor_Frequency, LeftMotor.Motor_SpeedCalc, LeftMotor.Motor_Error, LeftMotor.Motor_Speed #print RightMotor.Motor_PWM, RightMotor.Motor_Speed except (KeyboardInterrupt, Exception, AbortProgram, modbus_tk.modbus.ModbusError) as e: sys.stdout.write('bye-bye\r\n') # LeftMotor.Set_Target_Speed(0) # RightMotor.Set_Target_Speed(0) left_motor.close() right_motor.close() dev_pro.close() dev_joy.close() master.close() raise finally: pass
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!")