test_motor_board.py
Go to the documentation of this file.
1 #!/usr/bin/env python3
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 threading
42 import smbus
43 
44 # simple version string
45 g_version = "20210215"
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  threading.Thread(target=keyboard_thread, args=(keyBuf,)).start()
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  threading.Thread(target=keyboard_thread, args=(intKeys,)).start()
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(" p - Query PID control loop parameters O - Query firmware hw options")
304  logAlways(" D - Query range of registers for 32bit vals. S - Set word value for any register. Reg in hex, value as decimal")
305  logAlways(" q - Query a 16 bit word value from register. Q - Query 32 bit register value")
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  threading.Thread(target=keyboard_thread, args=(intKeys,)).start()
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 == 'D': # Do register dump of a range of registers
487  logAlways("Query any control register to any value up to one long word size")
488  cmdFirstRegAsHex = raw_input("Enter first control register number in hex: ")
489  cmdFirstRegNumber = int(cmdFirstRegAsHex,16)
490  cmdLastRegAsHex = raw_input("Enter last control register number in hex: ")
491  cmdLastRegNumber = int(cmdLastRegAsHex,16)
492  print("Dump MCB from hex Reg ", cmdFirstRegAsHex,"[",cmdFirstRegNumber,"] to hex Reg ", cmdLastRegAsHex,"[",cmdLastRegNumber,"]")
493  for reg in range(cmdFirstRegNumber, cmdLastRegNumber, 1):
494  queryBytes = [ 0x7e, 0x3a, 0x34, 0, 0, 0, 0 ]
495  queryBytes[2] = reg
496  pktCksum = calcPacketCksum(queryBytes)
497  queryBytes.append(pktCksum)
498  ser.flushInput()
499  ser.write(queryBytes)
500  hexRegValue = '{:02x}'.format(reg)
501  registerValue = fetchReplyLongWord(ser, '3c', hexRegValue)
502  integerValue = int(registerValue,16)
503  if integerValue > 2147483647:
504  integerValue = (4294967296 - integerValue) * -1
505  print("Reg ", hexRegValue, " value = ", registerValue, " hex : or dec ", integerValue)
506 
507  time.sleep(0.02)
508 
509  if input == 'E': # Enable ESET stop speed feature in firmware
510  logAlways("Enable firmware ESET stop safety feature")
511  queryBytes = [ 0x7e, 0x3a, 0x33, 0, 0, 0, 0 ]
512  pktCksum = calcPacketCksum(queryBytes)
513  queryBytes.append(pktCksum)
514  ser.flushInput()
515  ser.write(queryBytes)
516  estopEnableState = fetchReplyByte(ser, '3c', '33')
517  print("Prior ESTOP enable setting was ", estopEnableState)
518  time.sleep(0.02)
519  cmdPacket = formMagniParamSetMessage(0x33, 1)
520  ser.write(cmdPacket)
521  logAlways("Enabled firmware ESET stop safety feature")
522 
523  if input == 'i':
524  logAlways("Fetch Robot type ID")
525  queryBytes = [ 0x7e, 0x3a, 0x31, 0, 0, 0, 0 ]
526  pktCksum = calcPacketCksum(queryBytes)
527  queryBytes.append(pktCksum)
528  ser.flushInput()
529  ser.write(queryBytes)
530  robotTypeId = fetchReplyByte(ser, '3c', '31')
531  print("Robot type ID is ", robotTypeId)
532  time.sleep(0.02)
533 
534  if input == 'l':
535  logAlways("Rotate left ")
536  rightSpeed = 6
537  leftSpeed = 15
538  nextInput = setSpeedTillKeypress(ser, rightSpeed, leftSpeed)
539 
540  if input == 'm':
541  logAlways("Fetch motor controller motor power state")
542  queryBytes = [ 0x7e, 0x3a, 0x32, 0, 0, 0, 0 ]
543  pktCksum = calcPacketCksum(queryBytes)
544  queryBytes.append(pktCksum)
545  ser.flushInput()
546  ser.write(queryBytes)
547  motPowState = fetchReplyByte(ser, '3c', '32')
548  print("Motor controller things motor power state is ", motPowState)
549  time.sleep(0.02)
550 
551  if input == 'r':
552  logAlways("Rotate right ")
553  rightSpeed = 15
554  leftSpeed = 6
555  nextInput = setSpeedTillKeypress(ser, rightSpeed, leftSpeed)
556 
557  if input == 'q': # query any register to any value
558  logAlways("Query any control register to any value up to one word size")
559  cmdRegAsHex = raw_input("Enter control register number in hex: ")
560  cmdRegNumber = int(cmdRegAsHex,16)
561  queryBytes = [ 0x7e, 0x3a, 0x34, 0, 0, 0, 0 ]
562  queryBytes[2] = cmdRegNumber
563  pktCksum = calcPacketCksum(queryBytes)
564  queryBytes.append(pktCksum)
565  ser.flushInput()
566  ser.write(queryBytes)
567  registerValue = fetchReplyWord(ser, '3c', cmdRegAsHex)
568  print("Register was set to ", int(registerValue,16), " decimal", registerValue, " hex")
569  time.sleep(0.02)
570 
571  if input == 'O': # query the firmware options register set of bits
572  logAlways("Query the current hardware option bit settings")
573  cmdRegAsHex = '38'
574  cmdRegNumber = int(cmdRegAsHex,16)
575  queryBytes = [ 0x7e, 0x3a, 0x34, 0, 0, 0, 0 ]
576  queryBytes[2] = cmdRegNumber
577  pktCksum = calcPacketCksum(queryBytes)
578  queryBytes.append(pktCksum)
579  ser.flushInput()
580  ser.write(queryBytes)
581  logAlways("24")
582  registerValue = fetchReplyLongWord(ser, '3c', cmdRegAsHex)
583  logAlways("26")
584  print("Hardware option bits are set to ", int(registerValue,16), " decimal", registerValue, " hex")
585  if ((int(registerValue,16) & 0x01) != 0):
586  print (" High resolution encoders")
587  else:
588  print (" Standard resolution encoders")
589  if ((int(registerValue,16) & 0x02) != 0):
590  print (" Thin gearless wheels")
591  else:
592  print (" Standard wheels")
593  if ((int(registerValue,16) & 0x04) != 0):
594  print (" Reverse the wheel direction")
595  time.sleep(0.02)
596 
597  if input == 'Q': # query any register for a Long (32 bit value)
598  logAlways("Query any control register to any value up to one long word size")
599  cmdRegAsHex = raw_input("Enter control register number in hex: ")
600  cmdRegNumber = int(cmdRegAsHex,16)
601  queryBytes = [ 0x7e, 0x3a, 0x34, 0, 0, 0, 0 ]
602  queryBytes[2] = cmdRegNumber
603  pktCksum = calcPacketCksum(queryBytes)
604  queryBytes.append(pktCksum)
605  ser.flushInput()
606  ser.write(queryBytes)
607  registerValue = fetchReplyLongWord(ser, '3c', cmdRegAsHex)
608  integerValue = int(registerValue,16)
609  if integerValue > 2147483647:
610  integerValue = (4294967296 - integerValue) * -1
611  print("Reg ", cmdRegAsHex, " value = ", registerValue, " hex : or dec ", integerValue)
612  time.sleep(0.02)
613 
614  if input == 'S': # Set any register to any value
615  logAlways("Set any control register to any value up to one word size")
616  cmdRegAsHex = raw_input("Enter control register number in as hex digits: ")
617  cmdRegValue = raw_input("Enter control register value to be set in decimal: ")
618  cmdRegNumber = int(cmdRegAsHex,16)
619  cmdPacket = formMagniParamSetMessage(cmdRegNumber, cmdRegValue)
620  ser.write(cmdPacket)
621  time.sleep(0.02)
622  # nextInput = setSpeedTillKeypress(ser, lastSpeed, lastSpeed)
623 
624  if input == 'x':
625  logAlways("Exit after sending stop command")
626  cmdPacket = formMagniSpeedMessage(0, 0)
627  ser.write(cmdPacket)
628  exit()
629 
630  if input == 'o': # Read the option bits and board rev and if motor power is on or not
631  # i2c address of PCF8574 on the motor controller board
632  PCF8574 = 0x20
633 
634  logAlways("Query hardware version from I2C interface on motor controller board")
635  i2cbus = smbus.SMBus(1)
636  print ("Setup 8-bit I2C port to set as inputs. Also detect if port is present")
637  portPresent = 0
638  try:
639  i2cbus.write_byte(PCF8574,0xff)
640  portPresent = 1
641  time.sleep(0.2)
642  except Exception:
643  print ("Did not detect 8-bit port which is only on rev 5.0 and later boards OR I2C failure")
644 
645  if (portPresent == 1):
646  inputPortBits = i2cbus.read_byte(PCF8574)
647  # print ("Port returned: ", inputPortBits, " decimal")
648  if ((inputPortBits & 0x80) == 0):
649  print ("Motor power is OFF")
650  else:
651  print ("Motor power is ON")
652  # The 4 board revision bits are negative logic.
653  # They are 0x0E for binary 1 which is board revision 5.0
654  # We will only change the revision when hardware capabilities are different
655  boardRev = 49 + (15 - (inputPortBits & 0x0f))
656  print ("Motor Controller Board Revision is: ", boardRev)
657  optionBits = (inputPortBits & 0x70) >> 4
658  print ("Option jumper block is set to: (install a jumper sets a bit to 0)", optionBits)
659 
660  if input == 'p':
661  logAlways("Fetch PID Factors")
662  # send queries to fetch PID cooeficients
663  queryPid = [ 0x7e, 0x3a, 0x1b, 0, 0, 0, 0 ]
664  pktCksum = calcPacketCksum(queryPid)
665  queryPid.append(pktCksum)
666  ser.flushInput()
667  ser.write(queryPid)
668  pidReg = fetchReplyWord(ser, '3c', '1b')
669  print(" P (1b) = ", int(pidReg,16), " [", pidReg, " hex]")
670  queryPid = [ 0x7e, 0x3a, 0x1c, 0, 0, 0, 0 ]
671  pktCksum = calcPacketCksum(queryPid)
672  queryPid.append(pktCksum)
673  ser.flushInput()
674  ser.write(queryPid)
675  pidReg = fetchReplyWord(ser, '3c', '1c')
676  print(" I (1c) = ", int(pidReg,16), " [", pidReg, " hex]")
677  queryPid = [ 0x7e, 0x3a, 0x1d, 0, 0, 0, 0 ]
678  pktCksum = calcPacketCksum(queryPid)
679  queryPid.append(pktCksum)
680  ser.flushInput()
681  ser.write(queryPid)
682  pidReg = fetchReplyWord(ser, '3c', '1d')
683  print(" D (1d) = ", int(pidReg,16), " [", pidReg, " hex]")
684  queryPid = [ 0x7e, 0x3a, 0x37, 0, 0, 0, 0 ]
685  pktCksum = calcPacketCksum(queryPid)
686  queryPid.append(pktCksum)
687  ser.flushInput()
688  ser.write(queryPid)
689  pidReg = fetchReplyWord(ser, '3c', '37')
690  print(" MaxPWM (37)= ", int(pidReg,16), " [", pidReg, " hex]")
691  time.sleep(0.02)
692 
693  if input == 'v':
694  logAlways("Fetch software and hardware version information")
695  # send query for the firmware version
696  queryVersion = [ 0x7e, 0x3a, 0x22, 0, 0, 0, 0 ]
697  pktCksum = calcPacketCksum(queryVersion)
698  queryVersion.append(pktCksum)
699  ser.flushInput()
700  ser.write(queryVersion)
701  fwRev = fetchReplyByte(ser, '3c', '22')
702  fwRevInt = int(fwRev,16)
703  print("fw revision ", fwRevInt)
704  if fwRevInt >= 35:
705  # return daycode register if firmware version supports it
706  queryVersion = [ 0x7e, 0x3a, 0x3a, 0, 0, 0, 0 ]
707  pktCksum = calcPacketCksum(queryVersion)
708  queryVersion.append(pktCksum)
709  ser.flushInput()
710  ser.write(queryVersion)
711  registerValue = fetchReplyLongWord(ser, '3c', '3a')
712  print("fw daycode ", registerValue)
713  time.sleep(0.02)
714  # send query for the controller board hardware version
715  queryVersion = [ 0x7e, 0x3a, 0x21, 0, 0, 0, 0 ]
716  pktCksum = calcPacketCksum(queryVersion)
717  queryVersion.append(pktCksum)
718  ser.flushInput()
719  ser.write(queryVersion)
720  fwRev = fetchReplyByte(ser, '3c', '21')
721  boardRev = int(fwRev,16) / 10.0
722  print("hw revision ", boardRev)
723  time.sleep(0.02)
724 
725  except RuntimeError as e:
726  logAlways("Exception in magni_cmd: " + e.message)
727  except KeyboardInterrupt:
728  logAlways("terminated by keyboard interrupt! Zero the motor speed and exit")
729  lastSpeed = 0
730  cmdPacket = formMagniSpeedMessage(lastSpeed, lastSpeed)
731  ser.write(cmdPacket)
732  time.sleep(0.05)
733  except Exception:
734  logAlways("magni_cmd terminated.")
735  logAlways("NOTE: Program requires prior use of: sudo systemctl stop magni-base")
736 
737 
738 if __name__ == '__main__':
739  print("Running with script version: " + g_version)
740 
741  if str(len(sys.argv)) == "3":
742  if str(sys.argv[1]) == "--dev" or str(sys.argv[1]) == "--device":
743  g_serialDev = str(sys.argv[2])
744  print("Using serial device file: " + g_serialDev)
745 
746  try:
747  serCommander()
748  except RuntimeError as e:
749  logAlways("Exception in magni_cmd: " + e.message)
750  except Exception:
751  logAlways("magni_cmd terminated.")
test_motor_board.serCommander.__init__
def __init__(self)
Definition: test_motor_board.py:314
test_motor_board.fetchReplyLongWord
def fetchReplyLongWord(ser, cmdHex, regHex)
Definition: test_motor_board.py:235
test_motor_board.formMagniParamSetMessage
def formMagniParamSetMessage(registerNumber, paramValue)
Definition: test_motor_board.py:82
test_motor_board.logDebug
def logDebug(message)
Definition: test_motor_board.py:58
test_motor_board.runTillKeypress
def runTillKeypress(speed)
Definition: test_motor_board.py:123
test_motor_board.stopMotors
def stopMotors()
Definition: test_motor_board.py:142
test_motor_board.formMagniSpeedMessage
def formMagniSpeedMessage(rightSpeed, leftSpeed)
Definition: test_motor_board.py:102
serial::Serial
Definition: serial.h:147
test_motor_board.keyboard_thread
def keyboard_thread(intKeys)
Definition: test_motor_board.py:64
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.serCommander
Definition: test_motor_board.py:313
test_motor_board.logAlways
def logAlways(message)
Definition: test_motor_board.py:55
test_motor_board.setSpeedTillKeypress
def setSpeedTillKeypress(ser, speed1, speed2)
Definition: test_motor_board.py:284
test_motor_board.fetchReplyWord
def fetchReplyWord(ser, cmdHex, regHex)
Definition: test_motor_board.py:190
test_motor_board.fetchReplyByte
def fetchReplyByte(ser, cmdHex, regHex)
Definition: test_motor_board.py:148

ubiquity_motor
Author(s):
autogenerated on Thu Nov 16 2023 03:30:56