def sendMotor(ser, left, right):
motor = motor_pb2.Motor()
motor.speed_left = left
motor.speed_right = right
message = motor.SerializeToString()
ser.write('\x41') # A
ser.write('\x4E') # N
ser.write('\x53') # S
ser.write('\x49') # I
ser.write('\x02') # MOTOR
ser.write(pack('<H', len(message)))
ser.write(message)
print(motor)
print("HEXDUMP:" + ' '.join(hex(ord(ime)) for ime in message))
ser.flush()
def run(self):
state = 0
hexsize = ''
messagetype = 0
messagelength = 0
while True:
while state < 7:
c = self._ser.read()
if state == 0:
if ord(c) == 0x41:
state += 1
print ("A")
continue
else:
state = 0
if state == 1:
if ord(c) == 0x4E:
state += 1
print ("N")
continue
else:
state = 0
if state == 2:
if ord(c) == 0x53:
state += 1
print ("S")
continue
else:
state = 0
if state == 3:
if ord(c) == 0x49:
state += 1
print ("I")
continue
else:
state = 0
if state == 4:
messagetype = ord(c)
state += 1
print ("MessageType: %d" % messagetype)
continue
if state == 5:
hexsize = c
state += 1
print ("size")
continue
if state == 6:
hexsize += c
state += 1
messagelength = unpack('<H', hexsize)[0]
print ("size:%d" % messagelength)
continue
state = 0
message = self._ser.read(messagelength)
print ("Buffersize:%d" % len(message))
if messagetype == 4:
print ("STATUS:")
status = status_pb2.Status()
status.ParseFromString(message)
print (status)
self._queue.put(status)
if messagetype == 3:
print ("Sensor:")
sensor = sensor_pb2.Sensor()
sensor.ParseFromString(message)
print (sensor)
self._queue.put(sensor)
if messagetype == 2:
print ("Motor:")
motor = motor_pb2.Motor()
motor.ParseFromString(message)
print (motor)
self._queue.put(motor)
if messagetype == 1:
print ("Config:")
config = config_pb2.Config()
config.ParseFromString(message)
print (config)
self._queue.put(config)
def set_speed(self, left, right):
motor = motor_pb2.Motor()
motor.speed_left = left
motor.speed_right = right
self._sendQueue.put(motor)
