31,6 → 31,10 |
def __str__(self): |
return "Invalid Message type" |
|
class InvalidMsgLen(MkException): |
def __str__(self): |
return "Invalid Message Length" |
|
class NoResponse(MkException): |
def __init__(self, cmd): |
self.cmd = cmd |
144,7 → 148,41 |
int -= 0xFFFF |
return int |
|
class SettingsMsg: |
DATENREVISION = 80 |
IDX_INDEX = 0 |
IDX_STICK_P = 1 + 19 |
IDX_STICK_D = 1 + 20 |
IDX_NAME = 1 + 90 |
|
def __init__(self, msg): |
if (msg.cmd != 'Q'): |
raise InvalidMsgType |
if len(msg.data) != 105: |
raise InvalidMsgLen |
self.msg = msg |
|
def getSettings(self): |
return self.msg.data |
|
def getIndex(self): |
return self.getSetting(SettingsMsg.IDX_INDEX) |
|
def getSetting(self, settingIndex): |
return self.msg.data[settingIndex] |
|
def getName(self): |
name = "" |
for i in self.msg.data[SettingsMsg.IDX_NAME:]: |
if i==0: |
break |
name += chr(i) |
return name |
|
def setSetting(self, settingIndex, value): |
self.msg.data[settingIndex] = value |
|
|
class DebugDataMsg: |
IDX_ANALOG_ACCNICK = 2+2*2 |
IDX_ANALOG_ACCROLL = 2+2*3 |
157,16 → 195,16 |
self.msg = msg |
|
def getAccNick(self): |
return self.msg.data2SignedInt(DebugDataMsg.IDX_ANALOG_ACCNICK); |
return self.msg.data2SignedInt(DebugDataMsg.IDX_ANALOG_ACCNICK) |
|
def getAccRoll(self): |
return self.msg.data2SignedInt(DebugDataMsg.IDX_ANALOG_ACCROLL); |
return self.msg.data2SignedInt(DebugDataMsg.IDX_ANALOG_ACCROLL) |
|
def getCompassHeading(self): |
return self.msg.data2SignedInt(DebugDataMsg.IDX_ANALOG_COMPASS); |
return self.msg.data2SignedInt(DebugDataMsg.IDX_ANALOG_COMPASS) |
|
def getVoltage(self): |
return float(self.msg.data2SignedInt(DebugDataMsg.IDX_ANALOG_VOLTAGE))/10; |
return float(self.msg.data2SignedInt(DebugDataMsg.IDX_ANALOG_VOLTAGE))/10 |
|
class VibrationDataMsg: |
def __init__(self, msg): |
248,6 → 286,12 |
time.sleep(.5) |
# No reply expected... |
|
def sendSettings(self, settings): |
msg = MkMsg(address=MkComm.ADDRESS_FC, cmd='s', data=settings) |
self.sendLn(msg.generateMsg()) |
time.sleep(1) |
msg = self.waitForMsg('S') |
|
def getDebugMsg(self): |
self.serPort.flushInput() |
self.sendLn(self.getDebugMsgLn) |
255,6 → 299,14 |
msg = DebugDataMsg(msg) |
return msg |
|
def getSettingsMsg(self, index=0xFF): |
self.serPort.flushInput() |
msg = MkMsg(address=MkComm.ADDRESS_FC, cmd='q', data=[index]) |
self.sendLn(msg.generateMsg()) |
msg = self.waitForMsg('Q') |
msg = SettingsMsg(msg) |
return msg |
|
def getVersionMsg(self): |
self.sendLn(self.getVersionMsgLn) |
msg = self.waitForMsg('V') |
285,35 → 337,6 |
|
if __name__ == '__main__': |
try: |
|
## file = open('mklog.txt', "rbU") |
## |
## for line in file: |
## try: |
## msg = MkMsg(msg=line) |
## #print "Rec Addr:%02d cmd:%c data:" % (msg.address, msg.cmd), msg.data |
## if (msg.cmd == 'D'): |
## #print len(msg.data) |
## msg = DebugDataMsg(msg) |
## |
## #print "%d %d" % (msg.getAccNick(), msg.getAccRoll()) |
## |
## #val = abs(msg.getAccNick()) |
## #print "*************************************************************"[:val/10] |
## |
## print msg.getCompassHeading() |
## |
## except InvalidMsg: |
## pass |
## |
## file.close() |
## print "OK" |
|
## msg = MkMsg(address=1, cmd='a', data=[255, 254, 10, 11, 12]) |
## print msg.generateMsg() |
## msg2 = MkMsg(msg.generateMsg()) |
## print msg2.address, msg2.cmd, msg2.data |
|
comm = MkComm() |
comm.open(comPort="COM5") |
|
320,40 → 343,16 |
msg = comm.getVersionMsg() |
print "Version: %d.%d" % msg.getVersion() |
|
comm.doVibrationTest() |
msg = comm.getSettingsMsg() |
print "Index=",msg.getIndex() |
print "Name=",msg.getName() |
print "StickP=",msg.getSetting(SettingsMsg.IDX_STICK_P) |
print "StickD=",msg.getSetting(SettingsMsg.IDX_STICK_D) |
|
# msg = comm.getVersionMsg() |
# print "Version: %d.%d" % msg.getVersion() |
# |
# comm.setMotorTest([0,0,80,0]) |
# time.sleep(10) |
# |
# minN = 0 |
# maxN = 0 |
# minR = 0 |
# maxR = 0 |
# |
# time.clock() |
# for i in range(0,1000): |
# try: |
# msg = comm.getDebugMsg() |
# n = msg.getAccNick() |
# r = msg.getAccRoll() |
# print "%d,%d" % (n, r) |
# minN = min(minN, n) |
# maxN = max(maxN, n) |
# minR = min(minR, r) |
# maxR = max(maxR, r) |
# except Exception: |
# pass |
# |
# comm.setMotorTest([0,0,0,0]) |
# print time.clock() |
# print maxN-minN,maxR-minR |
msg.setSetting(SettingsMsg.IDX_STICK_P, msg.getSetting(SettingsMsg.IDX_STICK_P)+1) |
comm.sendSettings(msg.getSettings()) |
|
|
|
|
except Exception,e: |
print |
print "An error occured: ", e |