def setHighSpeed(self):
self.write(b"5")
highspeed = self.read(1)
if(highspeed == b"F"): # highspeed !!!
time.sleep(1)
self.connection.baudrate = 500000
self.write(b"\xAA\xAA\xAA\xAA\xAA\x99")
highspeed = self.read(2)
if highspeed == b"\xaaV":
if self.debuglevel >= 1:
print(_("Highspeed enabled"))
return True
else:
raise RP6ConnectionError(_("Could not setup high speed connection"))
def write(self, data):
self.connection.write(data)
if self.debuglevel >= 3:
outstr = _("OUT: ")
for char in data:
outstr += str(char) + " "
print(outstr)
def reset(self):
if self.debuglevel >= 1:
print(_("Resetting Robot"))
# set RTS on & off, to reset the robot
self.connection.setRTS(1)
time.sleep(1)
self.connection.setRTS(0)
# read a newline character
self.read(1)
# read the boot message
rp6boot = self.read(9)
if rp6boot == b"[RP6BOOT]":
# read another newline character
self.read(1)
return True
else:
raise RP6ConnectionError(_('Could not reset Robot %s') % str(rp6boot))
def read(self, length):
data = self.connection.read(length)
if self.debuglevel >= 3:
instr = _("IN: ")
for char in data:
instr += str(char) + " "
print(instr)
return data
def connect(self):
if self.debuglevel >= 1:
print(_("Connecting to %s") % self.port)
self.connection = serial.Serial(
port=self.port,
baudrate=38400,
timeout=1,
parity=serial.PARITY_NONE,
stopbits=serial.STOPBITS_ONE,
bytesize=serial.EIGHTBITS
)
def getType(self, getVoltage=False):
# send ASCII character with value of 072
self.write(b"H")
# read first part of next boot message
rp6boot = self.read(4)
if rp6boot == b"RP6:":
# read second part of boot message, to get the type
rp6read = self.read(5)
if rp6read == b"\x01\x01\x04\x05\x00":
self.type = "rp6"
if self.debuglevel >= 1:
print(_("RP6-Board connected"))
if getVoltage:
self.getBatteryVoltage()
elif rp6read == b"\x02\x01\x04\x03\x00":
self.type = "m32"
if self.debuglevel >= 1:
print(_("M32-Board connected"))
return self.type
else:
raise RP6ConnectionError(_("Could not determinate the type of the Robot"))
def parseHex(self, filename):
self.bindata = b""
if self.debuglevel >= 2:
print(_("Parsing Hexfile: %s") % filename)
file = open(filename, "rb")
for linenr, line in enumerate(file):
line = line[1:]
position = 0
checksum = 0;
while position < len(line) - 2:
checksum = checksum + int(line[position:position + 2], 16)
position += 2
if checksum % 256 != 0:
raise HexError(_("Incorrect Checksum at line %i") % linenr)
# cut line to data
line = line[8:-4]
position = 0
while position < len(line):
self.bindata += bytes([int(line[position:position + 2], 16)])
position += 2
def flash(self, bindata):
if self.debuglevel >= 1:
print(_("Initializing flash"))
# init flashing
self.write(b"I")
confirm = self.read(1)
if confirm != b"g":
return False
self.write(b"K")
confirm = self.read(1)
if confirm != b"[":
return False
flashlib = FlashLib(bindata)
blocknr = 0
if self.debuglevel >= 1:
print(_("Flashing"))
while flashlib.getblock(blocknr):
if self.debuglevel >= 2:
print(_("Flashing block: %i") % blocknr)
self.write(bytes([170]))
self.write(b"\x80")
self.write(b"\x00")
self.write(bytes([blocknr]))
flashdata = flashlib.getblock(blocknr)
self.write(flashdata[1])
self.write(bytes([(flashdata[0] & 255)]))
self.write(bytes([(flashdata[0] >> 8) & 255]))
self.write(b"\xAA")
confirm = self.read(3)
if confirm != b"B][":
raise FlashError(_("Error flashing Block %i") % blocknr)
blocknr += 1
if self.debuglevel >= 1:
print(_("Flashing finished"))
self.connection.baudrate = 38400
self.write(bytes([0]))
def getBatteryVoltage(self):
self.write(b"\x95")
value1 = ord(self.read(1))
value2 = ord(self.read(1))
# TODO check this
print(_("Voltage: %s V") % str((((value1 & 255) << 0) + ((value2 & 255) << 8)) / 102.4))