RobotZed/mbed.py at master · TeaPackCZ/RobotZed · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
from Logger import Logger
import signal
import sys
from time import sleep
import zmq
import serial

class mbed:
    def __init__(self):
        self.logger = Logger("MbedLog")

        self.InPorts = ["10210"]
        self.OutPorts = ["10201"]

        self.enabled = True
        self.connected = False
        signal.signal(signal.SIGINT, self.sigINT_Handler)
        zMQC = zmq.Context()

        self.subscriber = zMQC.socket(zmq.SUB)

        for port in self.InPorts:
            self.subscriber.connect('tcp://127.0.0.1:'+port)
            self.logger.save_line("Binded to port: " + port)
            sleep(0.5)
        self.subscriber.setsockopt(zmq.SUBSCRIBE, b"")

        self.publisher = zMQC.socket(zmq.PUB)

        for port in self.OutPorts:
            self.publisher.bind('tcp://127.0.0.1:'+port)
            self.logger.save_line("Connected to port: " + port)
            sleep(0.5)

        self.get_args()
        self.connect_serial()

        sleep(0.5)

    def get_args(self):
        if(len(sys.argv) < 2):
            print ("Usage: " + str(sys.argv[0]) + " <PATH TO TTY PORT>" +
                   "[opt: <BaudRate> <ZMQPort> <ID>]" )
            print ("Secondary use: " + str(sys.argv[0])
                   + " <PATH TO RAW LOGFILE>")
            sys.exit(0)

        if(len(sys.argv) >= 2):
            self.mserial = sys.argv[1]
            if(sys.argv[1].find("/dev/") >= 0):
                #normal run from physical device
                self.normalRun = True
            elif(sys.argv[1].find("Logs/") >= 0):
                # Run from LOG
                self.normalRun = False

        if(len(sys.argv) >= 3):
            self.mbaud = int(sys.argv[2])

        if(len(sys.argv) >= 4):
            self.zmqPort = str(sys.argv[3])

        if(len(sys.argv) >= 5):
            self.zmqID = str(sys.argv[4])

        print("Settings -> Serial: " + self.mserial + " speed: "
              + str(self.mbaud) + " ZMQ port: " + self.zmqPort
              + " ID: " + str(self.zmqID))

    def connect_serial(self):
        if not self.connected:
            if self.normalRun:
                try:
                    self.serial = serial.Serial(self.mserial, self.mbaud, timeout=0.05)
                    self.serial.bytesize = serial.EIGHTBITS
                    self.serial.parity = serial.PARITY_NONE
                    self.serial.stopbits = serial.STOPBITS_ONE

                    self.conected = True
                    print( "Connected to " + self.mserial)

                    if(self.serial.readable()):
                        self.serial.flush()
                        self.serial.readline()

                    return True
                except:
                    print( "Failed connecting to " + self.mserial)
                    return False
            else:
                try:
                    logfile = open(self.mserial,'rb')
                    self.LogData = []
                    for line in logfile:
                        self.LogData.append(line)
                    logfile.close()
                    return True
                except:
                    self.logger.save_line("Opening log file <" + self.mserial
                                          +"> failed")
                    return False

    def sigINT_Handler(self, signal, frame):
        print("\nMbed detected SigINT signal")
        self.logger.save_line("Signal SigINT detected")
        self.enabled = False
        for port in self.InPorts:
            self.subscriber.disconnect('tcp://127.0.0.1:'+port)
            self.logger.save_line("SUB Disconnected from local port: " + port)
        for port in self.OutPorts:
            self.publisher.disconnect('tcp://127.0.0.1:'+port)
            self.logger.save_line("PUB Disconnected from local port: " + port)
        if not self.connected:
            self.connected = False
            self.serial.flush()
            self.serial.close()
            self.logger.save_line("Serial on " + self.mserial + " disconnected.")
        sys.exit(0)

    def recvUpdate(self):
        waitingMSG = self.subscriber.poll(100,zmq.POLLIN)
        while(waitingMSG > 0):
            message = self.subscriber.recv_string()
            self.parse_zmq_message(message)
            waitingMSG = self.subscriber.poll(100,zmq.POLLIN)

        if(self.serial.readable()):
            message = self.serial.readline()
            self.parse_serial_message(message)

    def parse_zmq_message(self,data):
        self.logger.save_line("Receives ZMQ message: <" + data + ">")
        self.msgToWrite = data
        self.toWrite = True

    def parse_serial_message(self,data):
        self.logger.save_line("Receives serial message: <" + data + ">")

    def run(self):
        self.logger.save_line("mbed loop started...")
        print "Waitin for update..."
        self.toWrite = False
        sleep(1)
        while(self.enabled):
            self.recvUpdate()
            if(self.toWrite):
                self.serial.write(self.magToWrite)
            sleep(0.1)

M = mbed()
M.run()