test_motor_board.py
Go to the documentation of this file.
1 #!/usr/bin/pythmn -u
2 
3 #
4 # Command control direct to Ubiquity Robotics Magni Controller Board
5 #
6 # This is diagnostic code that can be usedto directly run the motors
7 # without ROS and all the overhead of joysticks and so on.
8 # The idea started as a simple serial controller to do basic tests.
9 #
10 # Usage:
11 # Stop the ROS magni control using: sudo systemctl stop magni-base
12 # sudo python magni_cmd.py # OPTIONALLY add --dev /dev/ttyUSB0 for other port
13 # Enter single keys followed by return to change speeds.
14 # Example: enter '1' for slow speed then enter 3 and speed goes to 3
15 # Change speeds like this and when done enter 'e' for exit or 's' for stop.
16 #
17 # The Magni uses a binary packet and this program uses only the speed command
18 # Packet Structure 0x7E CC RR AU AL BU BL SS
19 # Where 0x7E starts a packet and SS is cksum (CC -> B0) truncate 8 bit
20 # CC - Command MS Nibble is 3 for protocol version and LS Nibble:
21 # Read=0xA, Write=0xB, Response=0xC, Error=0xD
22 # RR - Register We only use the speed control which is 0x2A
23 # AUAL SpeedA signed 16-bit motor speed value for one motor
24 # BUBL SpeedB signed 16-bit motor speed value for one motor
25 #
26 # Test Rig Control
27 # You can not plug in the Raspberry Pi into the motor controller and can
28 # power the Raspberry Pi with an external 5V micro USB power supply.
29 # Next connect the Pi Pins 8,10,12 from your Pi to the controller CAREFULLY!
30 # By doing this you can run the PI from telnet and not have to worry about
31 # a bad controller board or power loss on controller board from hard reboot of Pi
32 #
33 # Developed by Mark Johnston, Aug 2018
34 #
35 
36 from __future__ import print_function
37 import sys
38 import time
39 import serial
40 import string
41 import thread
42 import smbus
43 
44 # simple version string
45 g_version = "20190703"
46 
47 # default serial device. An adapter in USB is often '/dev/ttyUSB0'
48 g_serialDev = '/dev/ttyAMA0'
49 
50 # This debug flag if set True enables prints and so on but cannot be used in production
51 g_debug = False
52 
53 
54 # Simple wrappers to prevent log overhead and use desired log functions for your system
55 def logAlways(message):
56  print(message)
57 
58 def logDebug(message):
59  global g_debug
60  if g_debug == True:
61  print(message)
62 
63 # Define an input thread that will pick up keyboard presses
64 def keyboard_thread(intKeys):
65  # logAlways("\nkeyboard input thread wait for key")
66  inChar = raw_input()
67  # logAlways(" keyboard input thread got a key")
68  intKeys.append(inChar)
69 
70 # Calculate a packet checksum for the given byte array
71 def calcPacketCksum(msgBytes):
72  cksum = 0
73  idx = 0
74  for c in msgBytes:
75  if idx > 0:
76  cksum = cksum + int(c)
77  idx = idx + 1
78  cksum = 0xff - (cksum & 0xff)
79  return cksum
80 
81 # Form a bytearray holding the LSB for a parameter set message to a given register
82 def formMagniParamSetMessage(registerNumber, paramValue):
83  # Start with a general message
84  paramSetCmd = [ 0x7e, 0x3b, 0x32, 0, 0, 0, 0 ]
85 
86  # fill in the register number
87  #paramSetCmd[2] = int(registerNumber) & int(0xff)
88  paramSetCmd[2] = registerNumber
89 
90  # fill in the least significant byte of the 32 bit value in the message
91  msByte = (int(paramValue)/int(256))
92  paramSetCmd[5] = msByte & 0xff
93  paramSetCmd[6] = int(paramValue) & 0xff
94 
95  # print("formMsg5 ", int(paramSetCmd[5]), " and lsb ", int(paramSetCmd[6]))
96  pktCksum = calcPacketCksum(paramSetCmd)
97  paramSetCmd.append(pktCksum)
98  return paramSetCmd
99 
100 # Form a bytearray holding a speed message with the left and right speed values
101 # This routine only handles -254 to 255 speeds but that is generally quite alright
102 def formMagniSpeedMessage(rightSpeed, leftSpeed):
103  # Start with a speed message for zero speed but without checksum
104  speedCmd = [ 0x7e, 0x3b, 0x2a, 0, 0, 0, 0 ]
105  # fill in speeds (just positive values so get this working!
106  if rightSpeed < 0:
107  speedCmd[3] = 0xff
108  speedCmd[4] = 0xff - ((-1 * rightSpeed) & 0xff)
109  else:
110  speedCmd[4] = rightSpeed & 0xff
111 
112  if leftSpeed < 0:
113  speedCmd[5] = 0xff
114  speedCmd[6] = 0xff - ((-1 * leftSpeed) & 0xff)
115  else:
116  speedCmd[6] = leftSpeed & 0xff
117  pktCksum = calcPacketCksum(speedCmd)
118  speedCmd.append(pktCksum)
119  return speedCmd
120 
121 # Run at the given speed till a key is entered
122 # This routine does not work yet so the program does crude inline like this
123 def runTillKeypress(speed):
124  print("runTillKeypress starting using speed ", int(speed))
125  keyBuf = []
126  thread.start_new_thread(keyboard_thread, (keyBuf,))
127  logAlways("runTillKeypress drop into loop")
128  while not keyBuf:
129  logAlways("runTillKeypress do the speed command")
130  packet = formMagniSpeedMessage(speed,speed)
131  print(packet)
132  ser.write(packet) # BUG!!! In this context the ser.write blows up yet it works in main
133  logAlways("runTillKeypress do the delay")
134  time.sleep(0.02)
135  logAlways("Finished with run command")
136  cmdPacket = formMagniSpeedMessage(0, 0)
137  ser.write(cmdPacket)
138  return keyBuf
139 
140 
141 # Since stop is so common we have a function to send all stop to the motors
142 def stopMotors():
143  cmdPacket = formMagniSpeedMessage(0, 0)
144  ser.write(cmdPacket)
145  return 0
146 
147 # Fetch a single byte from the fixed length reply for a parameter fetch of a given type
148 def fetchReplyByte(ser, cmdHex, regHex):
149  # get each byte of the reply and wait for the reply we need (totally nasty! why all the junk?)
150  # The Magni controller 'spews' forth loads of status all the time. VERY MESSY!
151  # We just read it all and search for the packet we need that starts with hex 7e 3c 22
152  time.sleep(0.02)
153  charsRead = 0
154  charState = 0
155  replyByte = '00'
156  read_byte = ser.read()
157  while read_byte is not None:
158  charsRead += 1
159  if (charsRead > 80):
160  print("fetchReplyByte: Too many reply chars ")
161  break
162  hexData = read_byte.encode('hex')
163  # print("char: ",hexData)
164  if (charState == 6):
165  replyByte = hexData
166  break
167  if (charState == 5):
168  charState += 1
169  if (charState == 4):
170  charState += 1
171  if (charState == 3):
172  charState += 1
173  if (charState == 2):
174  if (hexData == regHex):
175  charState += 1
176  else:
177  charState = 0 # if we are not on the packet we want reset the state
178  if (charState == 1):
179  if (hexData == cmdHex):
180  charState += 1
181  else:
182  charState = 0 # if we are not on the packet we want reset the state
183  if (charState == 0) and (hexData == '7e'):
184  charState += 1
185  read_byte = ser.read()
186  return replyByte
187 
188 
189 # Fetch a 16 bit value from the fixed length reply for a parameter fetch of a given type
190 def fetchReplyWord(ser, cmdHex, regHex):
191  # get each byte of the reply and wait for the reply we need (totally nasty! why all the junk?)
192  # The Magni controller 'spews' forth loads of status all the time. VERY MESSY!
193  # We just read it all and search for the packet we need that starts with hex 7e 3c 22
194  time.sleep(0.02)
195  charsRead = 0
196  charState = 0
197  replyMsb = '00'
198  replyWord = '0000'
199  read_byte = ser.read()
200  while read_byte is not None:
201  charsRead += 1
202  if (charsRead > 80):
203  print("fetchReplyWord: Too many reply chars ")
204  break
205  hexData = read_byte.encode('hex')
206  # print("char: ",hexData)
207  if (charState == 6):
208  replyWord = replyMsb + hexData
209  break
210  if (charState == 5):
211  replyMsb = hexData
212  charState += 1
213  if (charState == 4):
214  charState += 1
215  if (charState == 3):
216  charState += 1
217  if (charState == 2):
218  # if (hexData == int(regDec,16)):
219  if (hexData == regHex):
220  charState += 1
221  else:
222  charState = 0 # if we are not on the packet we want reset the state
223  if (charState == 1):
224  if (hexData == cmdHex):
225  charState += 1
226  else:
227  charState = 0 # if we are not on the packet we want reset the state
228  if (charState == 0) and (hexData == '7e'):
229  charState += 1
230  read_byte = ser.read()
231  return replyWord
232 
233 
234 # Fetch a 32 bit value from the fixed length reply for a parameter fetch of a given type
235 def fetchReplyLongWord(ser, cmdHex, regHex):
236  # get each byte of the reply and wait for the reply we need (totally nasty! why all the junk?)
237  # The Magni controller 'spews' forth loads of status all the time. VERY MESSY!
238  # We just read it all and search for the packet we need that starts with hex 7e 3c 22
239  time.sleep(0.02)
240  charsRead = 0
241  charState = 0
242  reply24 = '00'
243  reply16 = '00'
244  reply08 = '00'
245  replyLongWord = '0000'
246  read_byte = ser.read()
247  while read_byte is not None:
248  charsRead += 1
249  if (charsRead > 80):
250  print("fetchReplyWord: Too many reply chars ")
251  break
252  hexData = read_byte.encode('hex')
253  # print("char: ",hexData)
254  if (charState == 6):
255  replyLongWord = reply24 + reply16 + reply08 + hexData
256  break
257  if (charState == 5):
258  reply08 = hexData
259  charState += 1
260  if (charState == 4):
261  reply16 = hexData
262  charState += 1
263  if (charState == 3):
264  reply24 = hexData
265  charState += 1
266  if (charState == 2):
267  # if (hexData == int(regDec,16)):
268  if (hexData == regHex):
269  charState += 1
270  else:
271  charState = 0 # if we are not on the packet we want reset the state
272  if (charState == 1):
273  if (hexData == cmdHex):
274  charState += 1
275  else:
276  charState = 0 # if we are not on the packet we want reset the state
277  if (charState == 0) and (hexData == '7e'):
278  charState += 1
279  read_byte = ser.read()
280  return replyLongWord
281 
282 
283 # utility to set right and left wheel speeds then exit when key is hit
284 def setSpeedTillKeypress(ser, speed1, speed2):
285  intKeys = []
286  thread.start_new_thread(keyboard_thread, (intKeys,))
287  while not intKeys:
288  cmdPacket = formMagniSpeedMessage(speed1, speed2)
289  ser.write(cmdPacket)
290  time.sleep(0.02)
291  logAlways("Finished with run command")
292  cmdPacket = formMagniSpeedMessage(0, 0)
293  ser.write(cmdPacket)
294  keyInput = intKeys[0]
295  return keyInput
296 
297 def showHelp():
298  logAlways("ONLY USE THIS LOW LEVEL UTILITY WITH EXPLICIT HELP FROM Ubiquity Robotics SUPPORT!")
299  logAlways("ONLY RUN WHEN MAGNI IS STOPPED!. Use sudo systemctl stop magni-base.service")
300  logAlways("Commands: h or ? for help. v for versions of firmware and hardware. Use Control-C to quit or E")
301  logAlways("Speeds: Enter 0 - 9 fwd speed. n,N slow/fast reverse. s for 'any speed' or c cycle last fwd/reverse")
302  logAlways(" v - Query firmware and hw version setting o - Query 8-bit hardware option port with real board rev")
303  logAlways(" q - Query a 16 bit word value from register. Q - Query 32 bit register value")
304  logAlways(" S - Set a word value from for any register. Enter reg as hex and value as decimal")
305  logAlways(" 21 = Hardware Rev (50 = 5.0) 22 = Firmware version")
306  logAlways(" 32 = Query if firmware thinks motors active 33 = Set to 1 to enable any exit of ESTOP feature")
307  logAlways(" 34 = Set to max PID threshold where pre rev 5.0 boards did a safer ESTOP release. 0 to disable")
308  logAlways(" 35 = Set to the max forward limit speed 36 - Set to a max negative reverse limit speed")
309  logAlways(" 37 = Set max PWM setting [250] ")
310  return 0
311 
312 
313 class serCommander():
314  def __init__(self):
315  global g_serialDev
316  intKeys = []
317  keyBuf = []
318 
319  serialDev = g_serialDev
320 
321  # The bits we see here are placed in globals by notification callback
322 
323  nextInput=''
324  lastSpeed=0
325  lastNegativeSpeed=0
326  cycleOnPeriod=40 # cycles of constant running or time motor is ON for cycle test
327  cycleOffPeriod=40 # cycles of stopped running or time motor is OFF for cycle test
328 
329  # start thread to pick up keyboard presses
330  intKeys = []
331  nextCmd = ''
332 
333  # Magni Rasperry Pi serial port values
334  # ser = serial.Serial('/dev/ttyS0', 38400, 8, 'N', 1, timeout=1)
335  # Native Pi3B+ Ubiquity Robotics serial port Gnd=6 Tx=8 Rx=10 (+3.3V = 1)
336  # ser = serial.Serial('/dev/ttyAMA0', 38400, 8, 'N', 1, timeout=1)
337 
338  # First USB serial port (safer for reasons of static and power blowouts
339  print("Start Serial port using device ", serialDev)
340  try:
341  ser = serial.Serial(serialDev, 38400, 8, 'N', 1, timeout=1)
342  except Exception:
343  logAlways("Unable to open serial port. Verify correct port was specified")
344  exit()
345 
346  logAlways("Serial port opened")
347 
348  # always set speed to zero at the start to initialize motor controller zero point
349  logAlways("Set motors to run at 0.0 MPs")
350  lastSpeed = 0
351  cmdPacket = formMagniSpeedMessage(lastSpeed, lastSpeed)
352 
353  try:
354  ser.write(cmdPacket)
355  except Exception:
356  logAlways("Unable to properly send the command to Magni. This pregram canno run with Magni running.")
357  logAlways("Did you stop main software using sudo systemctl stop magni-base.service")
358  exit()
359 
360  time.sleep(0.05)
361  logAlways("Finished with run command")
362 
363  showHelp()
364 
365  try:
366  while True :
367 
368  # clear out interrupt keys if any are present
369  intKeys = []
370 
371  # get keyboard input
372  if nextInput == '':
373  input = raw_input(">> ")
374  else:
375  input = nextInput
376  nextInput = ''
377 
378  # Python 3 users
379  # input = input(">> ")
380 
381  if input == 'h':
382  showHelp()
383  if input == '?':
384  showHelp()
385 
386  if input == '0':
387  logAlways("Run at 0.0 MPs till a key is pressed")
388  lastSpeed = 0
389  nextInput = setSpeedTillKeypress(ser, lastSpeed, lastSpeed)
390 
391  if input == '1':
392  logAlways("Run at 0.1 MPs till a key is pressed")
393  lastSpeed = 5
394  nextInput = setSpeedTillKeypress(ser, lastSpeed, lastSpeed)
395 
396  if input == '2':
397  logAlways("Run at 0.2 MPs till a key is pressed")
398  lastSpeed = 10
399  nextInput = setSpeedTillKeypress(ser, lastSpeed, lastSpeed)
400 
401  if input == '3':
402  logAlways("Run at 0.3 MPs till a key is pressed")
403  lastSpeed = 24
404  nextInput = setSpeedTillKeypress(ser, lastSpeed, lastSpeed)
405 
406  if input == '4':
407  logAlways("Run at 0.4 MPs till a key is pressed")
408  lastSpeed = 32
409  nextInput = setSpeedTillKeypress(ser, lastSpeed, lastSpeed)
410 
411  if input == '5':
412  logAlways("Run at 0.5 MPs till a key is pressed")
413  lastSpeed = 40
414  nextInput = setSpeedTillKeypress(ser, lastSpeed, lastSpeed)
415 
416  if input == '6':
417  logAlways("Run at 0.65 MPS or 1 rev per second")
418  lastSpeed = 48
419  nextInput = setSpeedTillKeypress(ser, lastSpeed, lastSpeed)
420 
421  if input == '7':
422  logAlways("Run at 0.5 MPs till a key is pressed")
423  lastSpeed = 56
424  nextInput = setSpeedTillKeypress(ser, lastSpeed, lastSpeed)
425 
426  if input == '8':
427  logAlways("Run at 0.5 MPs till a key is pressed")
428  lastSpeed = 64
429  nextInput = setSpeedTillKeypress(ser, lastSpeed, lastSpeed)
430 
431  if input == '9':
432  logAlways("Run FAST till a key is pressed")
433  lastSpeed = 72
434  nextInput = setSpeedTillKeypress(ser, lastSpeed, lastSpeed)
435 
436  if input == 's':
437  logAlways("Set speed to any value")
438  lastSpeed = int(raw_input("Enter peed value 0-255 max integer: "))
439  nextInput = setSpeedTillKeypress(ser, lastSpeed, lastSpeed)
440 
441  if input == 'n':
442  logAlways("Run reverse using slow negative speed")
443  lastNegativeSpeed = -10
444  nextInput = setSpeedTillKeypress(ser, lastNegativeSpeed, lastNegativeSpeed)
445 
446  if input == 'N':
447  logAlways("Run reverse using fast negative speed")
448  lastNegativeSpeed = -95
449  nextInput = setSpeedTillKeypress(ser, lastNegativeSpeed, lastNegativeSpeed)
450 
451  if input == 'c': # Cycle from stop to last speed that was set over and over
452  logAlways("Cycle between last speed that was set to zero and back over and over")
453  thread.start_new_thread(keyboard_thread, (intKeys,))
454  while not intKeys:
455  loops = 1
456  print("Cycle to the ON speed for ", cycleOnPeriod, " cycles")
457  while not intKeys and loops < cycleOnPeriod:
458  cmdPacket = formMagniSpeedMessage(lastSpeed, lastSpeed)
459  ser.write(cmdPacket)
460  time.sleep(0.02)
461  loops = loops + 1;
462  loops = 1
463  print("Cycle to the OFF speed for ", cycleOffPeriod, " cycles")
464  while not intKeys and loops < cycleOffPeriod:
465  cmdPacket = formMagniSpeedMessage(lastNegativeSpeed, lastNegativeSpeed)
466  ser.write(cmdPacket)
467  time.sleep(0.02)
468  loops = loops + 1;
469  logAlways("Finished with run command")
470  cmdPacket = formMagniSpeedMessage(0, 0)
471  ser.write(cmdPacket)
472  nextInput = intKeys[0]
473 
474  if input == 'B':
475  cmdPacket = formMagniParamSetMessage(0x21, 50)
476  ser.write(cmdPacket)
477  logAlways("Forced Board Revision to rev 5.0")
478  time.sleep(0.02)
479 
480  if input == 'b':
481  cmdPacket = formMagniParamSetMessage(0x21, 49)
482  ser.write(cmdPacket)
483  logAlways("Forced Board Revision to rev 4.9")
484  time.sleep(0.02)
485 
486  if input == 'E': # Enable ESET stop speed feature in firmware
487  logAlways("Enable firmware ESET stop safety feature")
488  queryBytes = [ 0x7e, 0x3a, 0x33, 0, 0, 0, 0 ]
489  pktCksum = calcPacketCksum(queryBytes)
490  queryBytes.append(pktCksum)
491  ser.flushInput()
492  ser.write(queryBytes)
493  estopEnableState = fetchReplyByte(ser, '3c', '33')
494  print("Prior ESTOP enable setting was ", estopEnableState)
495  time.sleep(0.02)
496  cmdPacket = formMagniParamSetMessage(0x33, 1)
497  ser.write(cmdPacket)
498  logAlways("Enabled firmware ESET stop safety feature")
499 
500  if input == 'i':
501  logAlways("Fetch Robot type ID")
502  queryBytes = [ 0x7e, 0x3a, 0x31, 0, 0, 0, 0 ]
503  pktCksum = calcPacketCksum(queryBytes)
504  queryBytes.append(pktCksum)
505  ser.flushInput()
506  ser.write(queryBytes)
507  robotTypeId = fetchReplyByte(ser, '3c', '31')
508  print("Robot type ID is ", robotTypeId)
509  time.sleep(0.02)
510 
511  if input == 'l':
512  logAlways("Rotate left ")
513  rightSpeed = 6
514  leftSpeed = 15
515  nextInput = setSpeedTillKeypress(ser, rightSpeed, leftSpeed)
516 
517  if input == 'm':
518  logAlways("Fetch motor controller motor power state")
519  queryBytes = [ 0x7e, 0x3a, 0x32, 0, 0, 0, 0 ]
520  pktCksum = calcPacketCksum(queryBytes)
521  queryBytes.append(pktCksum)
522  ser.flushInput()
523  ser.write(queryBytes)
524  motPowState = fetchReplyByte(ser, '3c', '32')
525  print("Motor controller things motor power state is ", motPowState)
526  time.sleep(0.02)
527 
528  if input == 'r':
529  logAlways("Rotate right ")
530  rightSpeed = 15
531  leftSpeed = 6
532  nextInput = setSpeedTillKeypress(ser, rightSpeed, leftSpeed)
533 
534  if input == 'q': # query any register to any value
535  logAlways("Query any control register to any value up to one word size")
536  cmdRegAsHex = raw_input("Enter control register number in hex: ")
537  cmdRegNumber = int(cmdRegAsHex,16)
538  queryBytes = [ 0x7e, 0x3a, 0x34, 0, 0, 0, 0 ]
539  queryBytes[2] = cmdRegNumber
540  pktCksum = calcPacketCksum(queryBytes)
541  queryBytes.append(pktCksum)
542  ser.flushInput()
543  ser.write(queryBytes)
544  registerValue = fetchReplyWord(ser, '3c', cmdRegAsHex)
545  print("Register was set to ", int(registerValue,16), " decimal", registerValue, " hex")
546  time.sleep(0.02)
547 
548  if input == 'Q': # query any register for a Long (32 bit value)
549  logAlways("Query any control register to any value up to one long word size")
550  cmdRegAsHex = raw_input("Enter control register number in hex: ")
551  cmdRegNumber = int(cmdRegAsHex,16)
552  queryBytes = [ 0x7e, 0x3a, 0x34, 0, 0, 0, 0 ]
553  queryBytes[2] = cmdRegNumber
554  pktCksum = calcPacketCksum(queryBytes)
555  queryBytes.append(pktCksum)
556  ser.flushInput()
557  ser.write(queryBytes)
558  registerValue = fetchReplyLongWord(ser, '3c', cmdRegAsHex)
559  print("Register was set to ", int(registerValue,16), " decimal", registerValue, " hex")
560  time.sleep(0.02)
561 
562  if input == 'S': # Set any register to any value
563  logAlways("Set any control register to any value up to one word size")
564  cmdRegAsHex = raw_input("Enter control register number in as hex digits: ")
565  cmdRegValue = raw_input("Enter control register value to be set in decimal: ")
566  cmdRegNumber = int(cmdRegAsHex,16)
567  cmdPacket = formMagniParamSetMessage(cmdRegNumber, cmdRegValue)
568  ser.write(cmdPacket)
569  time.sleep(0.02)
570  nextInput = setSpeedTillKeypress(ser, lastSpeed, lastSpeed)
571 
572  if input == 'x':
573  logAlways("Exit after sending stop command")
574  cmdPacket = formMagniSpeedMessage(0, 0)
575  ser.write(cmdPacket)
576  exit()
577 
578  if input == 'o': # Read the option bits and board rev and if motor power is on or not
579  # i2c address of PCF8574 on the motor controller board
580  PCF8574 = 0x20
581 
582  logAlways("Query hardware version from I2C interface on motor controller board")
583  i2cbus = smbus.SMBus(1)
584  print ("Setup 8-bit I2C port to set as inputs. Also detect if port is present")
585  portPresent = 0
586  try:
587  i2cbus.write_byte(PCF8574,0xff)
588  portPresent = 1
589  time.sleep(0.2)
590  except Exception:
591  print ("Did not detect 8-bit port which is only on rev 5.0 and later boards OR I2C failure")
592 
593  if (portPresent == 1):
594  inputPortBits = i2cbus.read_byte(PCF8574)
595  # print ("Port returned: ", inputPortBits, " decimal")
596  if ((inputPortBits & 0x80) == 0):
597  print ("Motor power is OFF")
598  else:
599  print ("Motor power is ON")
600  # The 4 board revision bits are negative logic.
601  # They are 0x0E for binary 1 which is board revision 5.0
602  # We will only change the revision when hardware capabilities are different
603  boardRev = 49 + (15 - (inputPortBits & 0x0f))
604  print ("Motor Controller Board Revision is: ", boardRev)
605  optionBits = (inputPortBits & 0x70) >> 4
606  print ("Option jumper block is set to: (install a jumper sets a bit to 0)", optionBits)
607 
608 
609  if input == 'v':
610  logAlways("Fetch software and hardware version information")
611  # send query for the firmware version
612  queryVersion = [ 0x7e, 0x3a, 0x22, 0, 0, 0, 0 ]
613  pktCksum = calcPacketCksum(queryVersion)
614  queryVersion.append(pktCksum)
615  ser.flushInput()
616  ser.write(queryVersion)
617  fwRev = fetchReplyByte(ser, '3c', '22')
618  fwRevInt = int(fwRev,16)
619  print("fw revision ", fwRevInt)
620  if fwRevInt >= 35:
621  # return daycode register if firmware version supports it
622  queryVersion = [ 0x7e, 0x3a, 0x3a, 0, 0, 0, 0 ]
623  pktCksum = calcPacketCksum(queryVersion)
624  queryVersion.append(pktCksum)
625  ser.flushInput()
626  ser.write(queryVersion)
627  registerValue = fetchReplyLongWord(ser, '3c', '3a')
628  print("fw daycode ", registerValue)
629  time.sleep(0.02)
630  # send query for the controller board hardware version
631  queryVersion = [ 0x7e, 0x3a, 0x21, 0, 0, 0, 0 ]
632  pktCksum = calcPacketCksum(queryVersion)
633  queryVersion.append(pktCksum)
634  ser.flushInput()
635  ser.write(queryVersion)
636  fwRev = fetchReplyByte(ser, '3c', '21')
637  boardRev = int(fwRev,16) / 10.0
638  print("hw revision ", boardRev)
639  time.sleep(0.02)
640 
641  except RuntimeError,e:
642  logAlways("Exception in magni_cmd: " + e.message)
643  except KeyboardInterrupt:
644  logAlways("terminated by keyboard interrupt! Zero the motor speed and exit")
645  lastSpeed = 0
646  cmdPacket = formMagniSpeedMessage(lastSpeed, lastSpeed)
647  ser.write(cmdPacket)
648  time.sleep(0.05)
649  except Exception:
650  logAlways("magni_cmd terminated.")
651  logAlways("NOTE: Program requires prior use of: sudo systemctl stop magni-base")
652 
653 
654 if __name__ == '__main__':
655  print("Running with script version: " + g_version)
656 
657  if str(len(sys.argv)) == "3":
658  if str(sys.argv[1]) == "--dev":
659  g_serialDev = str(sys.argv[2])
660  print("Using serial device file: " + g_serialDev)
661 
662  try:
663  serCommander()
664  except RuntimeError,e:
665  logAlways("Exception in magni_cmd: " + e.message)
666  except Exception:
667  logAlways("magni_cmd terminated.")
test_motor_board.formMagniParamSetMessage
def formMagniParamSetMessage(registerNumber, paramValue)
Definition: test_motor_board.py:82
test_motor_board.keyboard_thread
def keyboard_thread(intKeys)
Definition: test_motor_board.py:64
test_motor_board.setSpeedTillKeypress
def setSpeedTillKeypress(ser, speed1, speed2)
Definition: test_motor_board.py:284
test_motor_board.runTillKeypress
def runTillKeypress(speed)
Definition: test_motor_board.py:123
test_motor_board.fetchReplyByte
def fetchReplyByte(ser, cmdHex, regHex)
Definition: test_motor_board.py:148
test_motor_board.formMagniSpeedMessage
def formMagniSpeedMessage(rightSpeed, leftSpeed)
Definition: test_motor_board.py:102
test_motor_board.fetchReplyLongWord
def fetchReplyLongWord(ser, cmdHex, regHex)
Definition: test_motor_board.py:235
test_motor_board.serCommander
Definition: test_motor_board.py:313
test_motor_board.stopMotors
def stopMotors()
Definition: test_motor_board.py:142
test_motor_board.serCommander.__init__
def __init__(self)
Definition: test_motor_board.py:314
test_motor_board.fetchReplyWord
def fetchReplyWord(ser, cmdHex, regHex)
Definition: test_motor_board.py:190
test_motor_board.calcPacketCksum
def calcPacketCksum(msgBytes)
Definition: test_motor_board.py:71
test_motor_board.showHelp
def showHelp()
Definition: test_motor_board.py:297
test_motor_board.logDebug
def logDebug(message)
Definition: test_motor_board.py:58
test_motor_board.logAlways
def logAlways(message)
Definition: test_motor_board.py:55

ubiquity_motor
Author(s):
autogenerated on Mon Feb 28 2022 23:57:06