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!")
