Rev 653 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 653 | Rev 696 | ||
---|---|---|---|
Line 28... | Line 28... | ||
28 | return "Invalid Arguments" |
28 | return "Invalid Arguments" |
Line 29... | Line 29... | ||
29 | 29 | ||
30 | class InvalidMsgType(MkException): |
30 | class InvalidMsgType(MkException): |
31 | def __str__(self): |
31 | def __str__(self): |
- | 32 | return "Invalid Message type" |
|
- | 33 | ||
- | 34 | class InvalidMsgLen(MkException): |
|
- | 35 | def __str__(self): |
|
Line 32... | Line 36... | ||
32 | return "Invalid Message type" |
36 | return "Invalid Message Length" |
33 | 37 | ||
34 | class NoResponse(MkException): |
38 | class NoResponse(MkException): |
Line 142... | Line 146... | ||
142 | int = self.data[index]+self.data[index+1]*256 |
146 | int = self.data[index]+self.data[index+1]*256 |
143 | if (int > 0xFFFF/2): |
147 | if (int > 0xFFFF/2): |
144 | int -= 0xFFFF |
148 | int -= 0xFFFF |
145 | return int |
149 | return int |
Line -... | Line 150... | ||
- | 150 | ||
- | 151 | class SettingsMsg: |
|
- | 152 | DATENREVISION = 80 |
|
- | 153 | IDX_INDEX = 0 |
|
- | 154 | IDX_STICK_P = 1 + 19 |
|
- | 155 | IDX_STICK_D = 1 + 20 |
|
- | 156 | IDX_NAME = 1 + 90 |
|
- | 157 | ||
- | 158 | def __init__(self, msg): |
|
- | 159 | if (msg.cmd != 'Q'): |
|
- | 160 | raise InvalidMsgType |
|
- | 161 | if len(msg.data) != 105: |
|
- | 162 | raise InvalidMsgLen |
|
- | 163 | self.msg = msg |
|
- | 164 | ||
- | 165 | def getSettings(self): |
|
- | 166 | return self.msg.data |
|
- | 167 | ||
- | 168 | def getIndex(self): |
|
- | 169 | return self.getSetting(SettingsMsg.IDX_INDEX) |
|
- | 170 | ||
- | 171 | def getSetting(self, settingIndex): |
|
- | 172 | return self.msg.data[settingIndex] |
|
- | 173 | ||
- | 174 | def getName(self): |
|
- | 175 | name = "" |
|
- | 176 | for i in self.msg.data[SettingsMsg.IDX_NAME:]: |
|
- | 177 | if i==0: |
|
- | 178 | break |
|
- | 179 | name += chr(i) |
|
- | 180 | return name |
|
- | 181 | ||
- | 182 | def setSetting(self, settingIndex, value): |
|
- | 183 | self.msg.data[settingIndex] = value |
|
Line 146... | Line 184... | ||
146 | 184 | ||
147 | 185 | ||
148 | class DebugDataMsg: |
186 | class DebugDataMsg: |
149 | IDX_ANALOG_ACCNICK = 2+2*2 |
187 | IDX_ANALOG_ACCNICK = 2+2*2 |
Line 155... | Line 193... | ||
155 | if (msg.cmd != 'D'): |
193 | if (msg.cmd != 'D'): |
156 | raise InvalidMsgType |
194 | raise InvalidMsgType |
157 | self.msg = msg |
195 | self.msg = msg |
Line 158... | Line 196... | ||
158 | 196 | ||
159 | def getAccNick(self): |
197 | def getAccNick(self): |
Line 160... | Line 198... | ||
160 | return self.msg.data2SignedInt(DebugDataMsg.IDX_ANALOG_ACCNICK); |
198 | return self.msg.data2SignedInt(DebugDataMsg.IDX_ANALOG_ACCNICK) |
161 | 199 | ||
Line 162... | Line 200... | ||
162 | def getAccRoll(self): |
200 | def getAccRoll(self): |
163 | return self.msg.data2SignedInt(DebugDataMsg.IDX_ANALOG_ACCROLL); |
201 | return self.msg.data2SignedInt(DebugDataMsg.IDX_ANALOG_ACCROLL) |
Line 164... | Line 202... | ||
164 | 202 | ||
165 | def getCompassHeading(self): |
203 | def getCompassHeading(self): |
Line 166... | Line 204... | ||
166 | return self.msg.data2SignedInt(DebugDataMsg.IDX_ANALOG_COMPASS); |
204 | return self.msg.data2SignedInt(DebugDataMsg.IDX_ANALOG_COMPASS) |
167 | 205 | ||
168 | def getVoltage(self): |
206 | def getVoltage(self): |
169 | return float(self.msg.data2SignedInt(DebugDataMsg.IDX_ANALOG_VOLTAGE))/10; |
207 | return float(self.msg.data2SignedInt(DebugDataMsg.IDX_ANALOG_VOLTAGE))/10 |
Line 244... | Line 282... | ||
244 | def sendNCRedirectUartFromFC(self): |
282 | def sendNCRedirectUartFromFC(self): |
245 | self.serPort.flushInput() |
283 | self.serPort.flushInput() |
246 | msg = MkMsg(address=MkComm.ADDRESS_NC, cmd='u', data=[0]) |
284 | msg = MkMsg(address=MkComm.ADDRESS_NC, cmd='u', data=[0]) |
247 | self.sendLn(msg.generateMsg()) |
285 | self.sendLn(msg.generateMsg()) |
248 | time.sleep(.5) |
286 | time.sleep(.5) |
249 | # No reply expected... |
287 | # No reply expected... |
- | 288 | ||
- | 289 | def sendSettings(self, settings): |
|
- | 290 | msg = MkMsg(address=MkComm.ADDRESS_FC, cmd='s', data=settings) |
|
- | 291 | self.sendLn(msg.generateMsg()) |
|
- | 292 | time.sleep(1) |
|
- | 293 | msg = self.waitForMsg('S') |
|
250 | 294 | ||
251 | def getDebugMsg(self): |
295 | def getDebugMsg(self): |
252 | self.serPort.flushInput() |
296 | self.serPort.flushInput() |
253 | self.sendLn(self.getDebugMsgLn) |
297 | self.sendLn(self.getDebugMsgLn) |
254 | msg = self.waitForMsg('D') |
298 | msg = self.waitForMsg('D') |
255 | msg = DebugDataMsg(msg) |
299 | msg = DebugDataMsg(msg) |
256 | return msg |
300 | return msg |
- | 301 | ||
- | 302 | def getSettingsMsg(self, index=0xFF): |
|
- | 303 | self.serPort.flushInput() |
|
- | 304 | msg = MkMsg(address=MkComm.ADDRESS_FC, cmd='q', data=[index]) |
|
- | 305 | self.sendLn(msg.generateMsg()) |
|
- | 306 | msg = self.waitForMsg('Q') |
|
- | 307 | msg = SettingsMsg(msg) |
|
- | 308 | return msg |
|
Line 257... | Line 309... | ||
257 | 309 | ||
258 | def getVersionMsg(self): |
310 | def getVersionMsg(self): |
259 | self.sendLn(self.getVersionMsgLn) |
311 | self.sendLn(self.getVersionMsgLn) |
260 | msg = self.waitForMsg('V') |
312 | msg = self.waitForMsg('V') |
Line 283... | Line 335... | ||
283 | 335 | ||
284 | 336 | ||
285 | - | ||
286 | if __name__ == '__main__': |
- | |
287 | try: |
- | |
288 | - | ||
289 | ## file = open('mklog.txt', "rbU") |
- | |
290 | ## |
- | |
291 | ## for line in file: |
- | |
292 | ## try: |
- | |
293 | ## msg = MkMsg(msg=line) |
- | |
294 | ## #print "Rec Addr:%02d cmd:%c data:" % (msg.address, msg.cmd), msg.data |
- | |
295 | ## if (msg.cmd == 'D'): |
- | |
296 | ## #print len(msg.data) |
- | |
297 | ## msg = DebugDataMsg(msg) |
- | |
298 | ## |
- | |
299 | ## #print "%d %d" % (msg.getAccNick(), msg.getAccRoll()) |
- | |
300 | ## |
- | |
301 | ## #val = abs(msg.getAccNick()) |
- | |
302 | ## #print "*************************************************************"[:val/10] |
- | |
303 | ## |
- | |
304 | ## print msg.getCompassHeading() |
- | |
305 | ## |
- | |
306 | ## except InvalidMsg: |
- | |
307 | ## pass |
- | |
308 | ## |
- | |
309 | ## file.close() |
- | |
310 | ## print "OK" |
- | |
311 | - | ||
312 | ## msg = MkMsg(address=1, cmd='a', data=[255, 254, 10, 11, 12]) |
- | |
313 | ## print msg.generateMsg() |
- | |
314 | ## msg2 = MkMsg(msg.generateMsg()) |
337 | |
315 | ## print msg2.address, msg2.cmd, msg2.data |
338 | if __name__ == '__main__': |
Line 316... | Line 339... | ||
316 | 339 | try: |
|
317 | comm = MkComm() |
340 | comm = MkComm() |
318 | comm.open(comPort="COM5") |
341 | comm.open(comPort="COM5") |
319 | - | ||
320 | msg = comm.getVersionMsg() |
- | |
321 | print "Version: %d.%d" % msg.getVersion() |
342 | |
322 | 343 | msg = comm.getVersionMsg() |
|
323 | comm.doVibrationTest() |
- | |
324 | - | ||
325 | # msg = comm.getVersionMsg() |
- | |
326 | # print "Version: %d.%d" % msg.getVersion() |
- | |
327 | # |
- | |
328 | # comm.setMotorTest([0,0,80,0]) |
- | |
329 | # time.sleep(10) |
- | |
330 | # |
- | |
331 | # minN = 0 |
- | |
332 | # maxN = 0 |
- | |
333 | # minR = 0 |
- | |
334 | # maxR = 0 |
- | |
335 | # |
- | |
336 | # time.clock() |
344 | print "Version: %d.%d" % msg.getVersion() |
337 | # for i in range(0,1000): |
345 | |
338 | # try: |
- | |
339 | # msg = comm.getDebugMsg() |
- | |
340 | # n = msg.getAccNick() |
- | |
341 | # r = msg.getAccRoll() |
346 | msg = comm.getSettingsMsg() |
342 | # print "%d,%d" % (n, r) |
- | |
343 | # minN = min(minN, n) |
- | |
344 | # maxN = max(maxN, n) |
- | |
345 | # minR = min(minR, r) |
347 | print "Index=",msg.getIndex() |
346 | # maxR = max(maxR, r) |
348 | print "Name=",msg.getName() |
347 | # except Exception: |
- | |
348 | # pass |
349 | print "StickP=",msg.getSetting(SettingsMsg.IDX_STICK_P) |
349 | # |
- | |
350 | # comm.setMotorTest([0,0,0,0]) |
- | |
Line 351... | Line 350... | ||
351 | # print time.clock() |
350 | print "StickD=",msg.getSetting(SettingsMsg.IDX_STICK_D) |
352 | # print maxN-minN,maxR-minR |
351 | |
353 | 352 | msg.setSetting(SettingsMsg.IDX_STICK_P, msg.getSetting(SettingsMsg.IDX_STICK_P)+1) |