[python] CAN Demo - 4-14-2021

Viewer

copydownloadembedprintName: CAN Demo - 4-14-2021
  1. ################################################################################
  2. ################################################################################
  3. #Servo Cylinder CAN Example code for Linux using 8devices Korlan USB2CAN device
  4. #For questions contact Tom Quartararo: [email protected]
  5.  
  6. ################################################################################
  7. ################################################################################
  8. import can
  9. import time
  10. import os
  11.  
  12. ################################################################################
  13. #############################**GLOBAL VARIABLES**###############################
  14. ################################################################################
  15. global previousStatus
  16. previousStatus = 0
  17.  
  18. ################################################################################
  19. #################################**CONSTANTS**##################################
  20. ################################################################################
  21. telDataTable ={
  22.     "A": "Status word byte 0 (LSB)",
  23.     "B": "Status word byte 1",
  24.     "C": "Status word byte 2",
  25.     "D": "Status word byte 3 (MSB)",
  26.     "E": "Average motor current over telemetry interval (0 to 32767) byte 0 (LSB)",
  27.     "F": "Average motor current over telemetry interval (0 to 32767) byte 1 (MSB)",
  28.     "G": "Servo Cylinder position, absolute encoder value (0 to 65535) byte 0 (LSB)",
  29.     "H": "Servo Cylinder position, absolute encoder value (0 to 65535) byte 1 (MSB)",
  30.     "I": "Position converted to input range (pMin to pMax) byte 0 (LSB)",
  31.     "J": "Position converted to input range (pMin to pMax) byte 1 (MSB)",
  32.     "K": "Latched high copy of status word byte 0 (LSB)",
  33.     "L": "Latched high copy of status word byte 1",
  34.     "M": "Latched high copy of status word byte 2",
  35.     "N": "Latched high copy of status word byte 3 (MSB)",
  36.     "O": "Latched low copy of status word byte 0 (LSB)",
  37.     "P": "Latched low copy of status word byte 1",
  38.     "Q": "Latched low copy of status word byte 2",
  39.     "R": "Latched low copy of status word byte 3 (MSB)",
  40.     "S": "8-bit position between physical stops (rPos to ePos)",
  41.     "T": "8-bit motor current 16-sample average of last 16 ms (0 to 255)",
  42.     "U": "8-bit bus voltage 0 VDC to +50 VDC (0 to 255)",
  43.     "V": "8-bit average motor current over telemetry interval (0 to 255)",
  44.     "W": "8-bit max motor current over telemetry interval (0 to 255)",
  45.     "X": "8-bit signed integer PCB temp sensor C (-50 to +127)",
  46.     "Y": "8-bit unsigned PCB temp sensor in deg_C where 0 = -50deg_C and 200 = +150deg_C (0 to 200) ",
  47.     "Z": "8-bit PCB relative humidity % (0 to 100)",
  48.     "m ": "Max motor current over telemetry interval (0 to 32767) byte 0 (LSB)",
  49.     "c ": "Max motor current over telemetry interval (0 to 32767) byte 1 (MSB)",
  50.     "p": "unitID byte 0 (LSB)",
  51.     "q": "unitID byte 1",
  52.     "r": "unitID byte 2",
  53.     "s": "unitID byte 3 (MSB)",
  54.     "t": "Target position, absolute encoder value byte 0 (LSB)",
  55.     "u": "Target position, absolute encoder value byte 1 (MSB)"
  56. }
  57.  
  58. statusWord = [
  59.     "Position at or beyond retracted physical stop rPos",
  60.     "Position at or beyond extended physical stop ePos",
  61.     "Position beyond retracted software limit spMin",
  62.     "Position beyond extended software limit spMax",
  63.     "Supply voltage low, motor in COAST (<6.75 VDC, 1 V hysteresis)",
  64.     "Supply voltage high, motor in dynamic brake  (>44.0 VDC, 2 V hysteresis)",
  65.     "Torque output greater than ovTorq limit",
  66.     "Torque command at maxTorq limit",
  67.     "Speed below “stop” threshold",
  68.     "Direction is extend",
  69.     "Position at target (position near target within posWin for posTime)",
  70.     "Following error (position error larger than fErrWin for time period fErrTime)",
  71.     "Command RX error (message not received in [rxTO * 800 µs])",
  72.     "Telemetry TX error (message not sent over full telemetry interval)",
  73.     "CAN position command input capped at low limit pMin",
  74.     "CAN position command input capped at upper limit pMax",
  75.     "Trajectory move active ",
  76.     "Heating active",
  77.     "Temperature at PCB greater than ovTemp value",
  78.     "Temperature at PCB less than unTemp value",
  79.     "Relative humidity at PCB greater than ovHumi value",
  80.     "Fatal error in CONFIG.TXT or HARDWARE.TXT",
  81.     "Fault output bit of DRV8323RS (Bridge Driver)",
  82.     "Erroneous warm reset of the CPU has occurred",
  83.     "opMode (CLI = 0, CAN = 1)",
  84.     "Interpolation enabled ",
  85.     "Heating enabled ",
  86.     "CAN bus module in passive mode ",
  87.     "USB connected",
  88.     "Opto input 1",
  89.     "Opto input 2",
  90.     "Opto input 3"
  91. ]
  92.  
  93. ################################################################################
  94. #################################**FUNCTIONS**##################################
  95. ################################################################################
  96. def formatRcvMsg( message ):
  97.     '''
  98.     Assumes telData is set to default telemetry ("KLMGHEFY")
  99.         message.data[0] = Latched high copy of status word byte 0 (LSB)
  100.         message.data[1] = Latched high copy of status word byte 1
  101.         message.data[2] = Latched high copy of status word byte 2
  102.         message.data[3] = Servo Cylinder position (0 to 65535) byte 0 (LSB)
  103.         message.data[4] = Servo Cylinder position (0 to 65535) byte 1 (MSB)
  104.         message.data[5] = Average motor current over telemetry interval (0 to 32767) byte 0 (LSB)
  105.         message.data[6] = Average motor current over telemetry interval (0 to 32767) byte 1 (MSB)
  106.         message.data[7] = 8-bit unsigned PCB temp sensor in deg_C where 0 = -50deg_C and 200 = +150deg_C (0 to 200)
  107.  
  108.     Use this function as a template for your telemetry requirements
  109.     '''
  110.     global previousStatus
  111.     print("")
  112.  
  113.     #reconstruct the status register from the 3x lowest bytes transmitted via telData
  114.     status = message.data[0] + (message.data[1] << 8) + (message.data[2] << 16)
  115.  
  116.     #check if the statusword has changed since last receipt and display any changes
  117.     xorStatus = status ^ previousStatus
  118.     if( xorStatus != 0 ):
  119.         print( "Status Word: ", hex(status) )
  120.         xorBitResults = [int(x) for x in '{:024b}'.format(xorStatus)] #convert xor'd result to 24-bit array
  121.         for k in range(len(xorBitResults)):
  122.             if( xorBitResults[k] == 1 ): #get the bits of register that changed from last time
  123.                 print( "\t", "Bit", len(xorBitResults) -1 - k, ":", statusWord[ len(xorBitResults) - 1 - k ] )
  124.     previousStatus = status
  125.  
  126.     #Write position/current/temperature telemetry to terminal
  127.     print( "Position Feedback: ", message.data[3] + (message.data[4] << 8) )
  128.     print( "Current Feedback: ", message.data[5] + (message.data[6] << 8) )
  129.     print( "Temperature Feedback: ", message.data[7] - 50.0, " deg_C" )
  130.  
  131. ################################################################################
  132. ####################################**MAIN**####################################
  133. ################################################################################
  134.  
  135. #Configure the USB2CAN device
  136. os.system("ip link set can2 up type can bitrate 250000 sample-point 0.875")
  137. time.sleep(0.1)
  138. bus = can.interface.Bus( bustype = 'socketcan', channel = 'can2', bitrate = 250000 )
  139. print('CAN port 1 configured.')
  140.  
  141. #set the Servo Cylinder's unitID
  142. actuatorID = 0x4
  143.  
  144. while 1:
  145.  
  146.     #Received Telemetry from the Actuator
  147.     rcvMsg = bus.recv( timeout = 0.005 )
  148.     if rcvMsg is not None: #if there's data available
  149.         formatRcvMsg( rcvMsg )
  150.  
  151.  
  152.     #Generating the actuator commands to send over CAN
  153.     pos = 5000 #enter your position generating function here (ranges from pMin to pMax, mapped to spMin/spMax)
  154.     posLowByte = pos & 0x00FF
  155.     posHighByte = (pos & 0xFF00) >> 8
  156.  
  157.     MT = 6000 #enter your current limiting function here
  158.     currLowByte = MT & 0x00FF
  159.     currHighByte = (MT & 0xFF00) >> 8
  160.  
  161.     #configure message to send to actuator (assumes <>() RX setting)
  162.     msg = can.Message( arbitration_id = actuatorID, data = [ posLowByte, posHighByte, currLowByte, currHighByte ], is_extended_id = True )
  163.  
  164.     #send the CAN message
  165.     try:
  166.         bus.send(msg)
  167.     except Exception as e:
  168.         print("CAN Transmit Error: ", e )
  169.  
  170.     time.sleep( 0.080 )
  171.  
  172.  
  173. ################################################################################
  174. ####################################**END**#####################################
  175. ################################################################################
  176.  

Editor

You can edit this paste and save as new:


File Description
  • CAN Demo - 4-14-2021
  • Paste Code
  • 14 Apr-2021
  • 8.43 Kb
You can Share it: