Servo Cylinder CAN Demo
unknown
python
14 days ago
8.6 kB
12
Indexable
################################################################################
################################################################################
#Servo Cylinder CAN Example code for Linux using 8devices Korlan USB2CAN device
#For questions contact Tom Quartararo: tom@ultramotion.com
################################################################################
################################################################################
import can
import time
import os
################################################################################
#############################**GLOBAL VARIABLES**###############################
################################################################################
global previousStatus
previousStatus = 0
################################################################################
#################################**CONSTANTS**##################################
################################################################################
telDataTable = {
"A": "Status word byte 0 (LSB)",
"B": "Status word byte 1",
"C": "Status word byte 2",
"D": "Status word byte 3 (MSB)",
"E": "Average motor current over telemetry interval (0 to 32767) byte 0 (LSB)",
"F": "Average motor current over telemetry interval (0 to 32767) byte 1 (MSB)",
"G": "Servo Cylinder position, absolute encoder value (0 to 65535) byte 0 (LSB)",
"H": "Servo Cylinder position, absolute encoder value (0 to 65535) byte 1 (MSB)",
"I": "Position converted to input range (pMin to pMax) byte 0 (LSB)",
"J": "Position converted to input range (pMin to pMax) byte 1 (MSB)",
"K": "Latched high copy of status word byte 0 (LSB)",
"L": "Latched high copy of status word byte 1",
"M": "Latched high copy of status word byte 2",
"N": "Latched high copy of status word byte 3 (MSB)",
"O": "Latched low copy of status word byte 0 (LSB)",
"P": "Latched low copy of status word byte 1",
"Q": "Latched low copy of status word byte 2",
"R": "Latched low copy of status word byte 3 (MSB)",
"S": "8-bit position between physical stops (rPos to ePos)",
"T": "8-bit motor current 16-sample average of last 16 ms (0 to 255)",
"U": "8-bit bus voltage 0 VDC to +50 VDC (0 to 255)",
"V": "8-bit average motor current over telemetry interval (0 to 255)",
"W": "8-bit max motor current over telemetry interval (0 to 255)",
"X": "8-bit signed integer PCB temp sensor C (-50 to +127)",
"Y": "8-bit unsigned PCB temp sensor in deg_C where 0 = -50deg_C and 200 = +150deg_C (0 to 200) ",
"Z": "8-bit PCB relative humidity % (0 to 100)",
"m ": "Max motor current over telemetry interval (0 to 32767) byte 0 (LSB)",
"c ": "Max motor current over telemetry interval (0 to 32767) byte 1 (MSB)",
"p": "unitID byte 0 (LSB)",
"q": "unitID byte 1",
"r": "unitID byte 2",
"s": "unitID byte 3 (MSB)",
"t": "Target position, absolute encoder value byte 0 (LSB)",
"u": "Target position, absolute encoder value byte 1 (MSB)"
}
statusWord = [
"Position at or beyond retracted physical stop rPos",
"Position at or beyond extended physical stop ePos",
"Position beyond retracted software limit spMin",
"Position beyond extended software limit spMax",
"Supply voltage low, motor in COAST (<6.75 VDC, 1 V hysteresis)",
"Supply voltage high, motor in dynamic brake (>44.0 VDC, 2 V hysteresis)",
"Torque output greater than ovTorq limit",
"Torque command at maxTorq limit",
"Speed below “stop” threshold",
"Direction is extend",
"Position at target (position near target within posWin for posTime)",
"Following error (position error larger than fErrWin for time period fErrTime)",
"Command RX error (message not received in [rxTO * 800 µs])",
"Telemetry TX error (message not sent over full telemetry interval)",
"CAN position command input capped at low limit pMin",
"CAN position command input capped at upper limit pMax",
"Trajectory move active ",
"Heating active",
"Temperature at PCB greater than ovTemp value",
"Temperature at PCB less than unTemp value",
"Relative humidity at PCB greater than ovHumi value",
"Fatal error in CONFIG.TXT or HARDWARE.TXT",
"Fault output bit of DRV8323RS (Bridge Driver)",
"Erroneous warm reset of the CPU has occurred",
"opMode (CLI = 0, CAN = 1)",
"Interpolation enabled ",
"Heating enabled ",
"CAN bus module in passive mode ",
"USB connected",
"Opto input 1",
"Opto input 2",
"Opto input 3"
]
################################################################################
#################################**FUNCTIONS**##################################
################################################################################
def formatRcvMsg( message ):
'''
Assumes telData is set to default telemetry ("KLMGHEFY")
message.data[0] = Latched high copy of status word byte 0 (LSB)
message.data[1] = Latched high copy of status word byte 1
message.data[2] = Latched high copy of status word byte 2
message.data[3] = Servo Cylinder position (0 to 65535) byte 0 (LSB)
message.data[4] = Servo Cylinder position (0 to 65535) byte 1 (MSB)
message.data[5] = Average motor current over telemetry interval (0 to 32767) byte 0 (LSB)
message.data[6] = Average motor current over telemetry interval (0 to 32767) byte 1 (MSB)
message.data[7] = 8-bit unsigned PCB temp sensor in deg_C where 0 = -50deg_C and 200 = +150deg_C (0 to 200)
Use this function as a template for your telemetry requirements
'''
global previousStatus
print("")
#reconstruct the status register from the 3x lowest bytes transmitted via telData
status = message.data[0] + (message.data[1] << 8) + (message.data[2] << 16)
#check if the statusword has changed since last receipt and display any changes
xorStatus = status ^ previousStatus
if( xorStatus != 0 ):
print( "Status Word: ", hex(status) )
xorBitResults = [int(x) for x in '{:024b}'.format(xorStatus)] #convert xor'd result to 24-bit array
for k in range(len(xorBitResults)):
if( xorBitResults[k] == 1 ): #get the bits of register that changed from last time
print( "\t", "Bit", len(xorBitResults) -1 - k, ":", statusWord[ len(xorBitResults) - 1 - k ] )
previousStatus = status
#Write position/current/temperature telemetry to terminal
print( "Position Feedback: ", message.data[3] + (message.data[4] << 8) )
print( "Current Feedback: ", message.data[5] + (message.data[6] << 8) )
print( "Temperature Feedback: ", message.data[7] - 50.0, " deg_C" )
################################################################################
####################################**MAIN**####################################
################################################################################
#Configure the USB2CAN device
os.system("ip link set can2 up type can bitrate 250000 sample-point 0.875")
time.sleep(0.1)
bus = can.interface.Bus( bustype = 'socketcan', channel = 'can2', bitrate = 250000 )
print('CAN port 1 configured.')
#set the Servo Cylinder's unitID
actuatorID = 0x4
while 1:
#Received Telemetry from the Actuator
rcvMsg = bus.recv( timeout = 0.005 )
if rcvMsg is not None: #if there's data available
formatRcvMsg( rcvMsg )
#Generating the actuator commands to send over CAN
pos = 5000 #enter your position generating function here (ranges from pMin to pMax, mapped to spMin/spMax)
posLowByte = pos & 0x00FF
posHighByte = (pos & 0xFF00) >> 8
MT = 6000 #enter your current limiting function here
currLowByte = MT & 0x00FF
currHighByte = (MT & 0xFF00) >> 8
#configure message to send to actuator (assumes <>() RX setting)
msg = can.Message( arbitration_id = actuatorID, data = [ posLowByte, posHighByte, currLowByte, currHighByte ], is_extended_id = True )
#send the CAN message
try:
bus.send(msg)
except Exception as e:
print("CAN Transmit Error: ", e )
time.sleep( 0.080 )
################################################################################
####################################**END**#####################################
################################################################################
Editor is loading...
Leave a Comment