Subversion Repositories Projects

Compare Revisions

Ignore whitespace Rev 695 → Rev 696

/VibrationTest/trunk/VibrationTest/mkProto.py
30,6 → 30,10
class InvalidMsgType(MkException):
def __str__(self):
return "Invalid Message type"
class InvalidMsgLen(MkException):
def __str__(self):
return "Invalid Message Length"
 
class NoResponse(MkException):
def __init__(self, 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):
246,8 → 284,14
msg = MkMsg(address=MkComm.ADDRESS_NC, cmd='u', data=[0])
self.sendLn(msg.generateMsg())
time.sleep(.5)
# No reply expected...
 
# 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)
254,6 → 298,14
msg = self.waitForMsg('D')
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)
285,75 → 337,22
 
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")
msg = comm.getVersionMsg()
print "Version: %d.%d" % msg.getVersion()
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.setSetting(SettingsMsg.IDX_STICK_P, msg.getSetting(SettingsMsg.IDX_STICK_P)+1)
comm.sendSettings(msg.getSettings())
 
comm.doVibrationTest()
 
# 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
 
 
 
except Exception,e:
print
print "An error occured: ", e