#%TITLE% ELMO.MAC
#
#%NAME%
# Macros to control ELMO motor controller through macro motor.
# Tested only with model "Whistle 01.01.10.00 31Dec2014B00G"
# using ethernet connection (configured by USB only)
# driving a single rotation stage.
#
#%CATEGORY% Positioning
#
#%DESCRIPTION%
#%DL%
#%DT%Configuring macro motors %DD%
# %DL%
# %DT% 1)
# You must have an entry in "MOTORS" table for each controller
# %DL%
# %DT% -
# The "DEVICE" field must be set to string "elmoeth"
# %DT% -
# The "ADDR" must be set to the IP adress + port like
# %B%elmoid111:5001%B%
#
# %DT% -
# The "NUM" should be set to
# %DT% -
# The "TYPE" field must be set to "Macro Motors"
# %XDL%
# %DT% 2)
# For each axis you must define a motor with:
# %DL%
# %DT% -
# The "Controller" field set to "MAC_MOT"
# %DT% -
# The "Unit" field, numbered from 0,
# must be set to the MOTORS entry.
# %DT% -
# The "Module" field is not used
# %DT% -
# The "Chan" field is not used
# %DT%
# In the additionnal parameter screen, the parameter
# "control_slave" can be set to 1 if the main motor
# has a slave driven by elmo controller
# %XDL%
# %XDL%
#%XDL%
#
#%DL%
#%DT%Controller mode (user_mode motor_par) %DD%
# %DL%
# %DT% Position control mode
# %DD% On spec %B%reconfig%B%, the control position is automatically set.
# The ELMO corresponding user_mode is 4 for linear motors (aux. encoder feedback loop) or
# 5 for rotary motors (main encoder feedback loop) depending on "Linear/Rotary" field in spec config.
# %DT% Speed control mode
# %DD% Correspond to ELMO user_mode = 2. In this mode, higher velocity can be reached but
# no control position commands can be executed (ie. no standard mv/mvr/...).
# %XDL%
#%XDL%
#
#%DL%
#%DT% Motor parameters implemented %DD%
# %DL%
# %DT% power
# %DD% Switch motor on(1) / off(0). On power ON, the command position is updated.
# If the %B%user_mode%B% is in control position (4 or 5), on power ON, the command position is updated.
# If ran outside config (ie. on a motor_par), read_motors(0x6) is called to update spec internals.
# %DT% enc_position
# %DD% Return actual encoder position
# %DT% user_mode
# %DD% Change the ELMO control mode (4/5: position control, 2: speed control). When switching to position control
# mode, the command position is updated. The read_motors(0x6) is executed to update spec internals.
# %DT% jog_speed
# %DD% Start a jog speed motion in (deg or mm)/sec. Stop it if speed = 0.
# %DT% jog_range
# %DD% Read speed range for jog motion
# %DT% enable_slave
# %DD% Temporary disable/enable slave control
# %XDL%
#%XDL%
#
#%DL%
#%DT% Homing procedure implemented %DD%
# %DL%
# %DT% chg_dial(mot, "home+")
# %DD% Jog motor in user positive direction until home switch. Sets the controller position (dial) to the
# position eventually defined in spec config as "home position" (0 by default)
# %DT% chg_dial(mot, "home-")
# %DD% Same procedure moving in negative direction
# %XDL%
#%XDL%
#%END%
#
constant ELMO_SLBAUD 115200
constant ELMO_ERR "ELMO ERROR: "
constant ELMO_CTLERR "ELMO CONTROLLER ERROR: "
constant MOT_MOVING 0x02
constant MOT_LIMP 0x04
constant MOT_LIMM 0x08
constant MOT_EMERGENCY 0X10
constant MOT_FAULT 0X20
constant MOT_OK 0X00
#%UU%
#%MDESC%
# Toggle on/off debug messages
#
def elmoethdebug '{
global ELMO_DEBUG
if (ELMO_DEBUG) {
rdef elmoeth__debug \'#\$#\'
print "ELMO debug mode is OFF"
ELMO_DEBUG=0
} else {
rdef elmoeth__debug \'print "ELMO: "\'
print "ELMO debug mode is ON"
ELMO_DEBUG=1
}
}'
def elmoethdebug_init '{
if(!(whatis("elmoeth__debug")&0x2)) {
global ELMO_DEBUG
rdef elmoeth__debug \'#\$#\'
ELMO_DEBUG= 0
}
}'
elmoethdebug_init
#%IU%
#%MDESC%
# MACRO MOTOR:
# Called by spec after reading the config file
#
def elmoeth_config(num,type,p1,p2,p3) '{
local sl ans mne rot
global ELMO_PAR[]
# Get the communication link
sl=elmoeth_ADDR
# p1==controller index p2==number of motors supported
if(type=="ctrl") {
# try connect
ans= sock_par(sl, "connect_udp")
if (!ans) {
print ELMO_ERR "cannot connect to " sl
return ".error."
}
# Check that the controller is alive
ans = elmoeth_query(sl,"VR")
if((ans=="") || (ans==-1)) {
print ELMO_ERR "missing controller"
return ".error."
}
print "ELMO controller: "ans
}
# p1==unit p2==module p3==channel
if(type=="mot") {
local mode slave power
mne= motor_mne(num)
ELMO_PAR[mne]["config"]= 1
# Check user-mode
mode= elmoeth_query(sl,"UM")
if (mode != 5) {
print "ELMO motor " mne" : Activate position mode"
ans= elmoeth_query(sl, "UM=5")
}
# Activate slave if needed
slave= motor_par(num, "control_slave")
if (slave == 1) {
print "ELMO motor " mne " : Init slave"
ans= elmoeth_query(sl, "OB[1]=1")
}
# Check closed loop on
power= elmoeth_query(sl,"MO")
if (power != 1) {
print "ELMO motor " mne" : Activate closed loop"
ans= elmoeth_query(sl, "MO=1")
}
# reset jog speed
ELMO_PAR[mne]["jog"]= 0
ELMO_PAR[mne]["config"]= 0
print "ELMO motor " mne " : closed loop activated"
}
}'
#%IU%
#%MDESC%
# MACRO MOTOR:
# Called by spec after reading the config file, after calling _config()
# and only if parameters are set in the config file for a motor.
#
def elmoeth_par(num,key,todo,p1) '{
local mne
local sl
# get the motor mnemonic string
mne=motor_mne(num)
# Get the communication link
sl=motor_par(num,"address")
# return new motor_par() argins handled
if (key == "?" && todo == "get") {
return("power,user_mode,enc_position,jog_speed,jog_range,enable_slave")
}
if (key == "power") {
local sta ans _pos_sync
ans= elmoeth_query(sl, "SO")
if (sscanf(ans, "%d", sta) != 1) {
print ELMO_ERR "unable to get servo status"
return (-1)
}
if (todo == "set") {
if (motor_par(mne, "control_slave") == 1) {
if (p1==0) {
print "ELMO motor "mne" : disable slave control"
elmoeth_query(sl, "OB[1]=0")
} else {
print "ELMO motor "mne" : enable slave control"
elmoeth_query(sl, "OB[1]=1")
}
}
elmoeth_query(sl, sprintf("MO=%d", (p1==1?1:0)))
_pos_sync= 0
if ((sta==0)&&(p1==1)) {
_pos_sync= 1
}
ans= elmoeth_query(sl, "SO")
if (sscanf(ans, "%d", sta) != 1) {
print ELMO_ERR "unable to get servo status"
return (-1)
}
if (p1==1) {
while (sta != 1) {
print "ELMO motor " mne" : Waiting servo mode ON ..."
sleep(0.1)
ans= elmoeth_query(sl, "SO")
if (sscanf(ans, "%d", sta) != 1) {
print ELMO_ERR "unable to get servo status"
return (-1)
}
}
print "ELMO motor " mne" : Servo mode activated"
if (ELMO_PAR["srot"]["mode"] != 2) {
if ((todo == "set")&&(_pos_sync==1)) {
elmoeth_query(sl, "PA=DV[3]")
if (!ELMO_PAR[mne]["config"]) {
read_motors(0x6)
}
}
}
}
}
return (sta==1)
}
if (key == "user_mode") {
local ans mode
local newmode ncmd cmds[] vals[]
if (todo == "set") {
mode= int(p1)
ncmd= 0
cmds[ncmd]= "MO=0"
cmds[++ncmd]= sprintf("UM=%d", mode)
if (mode==2) {
cmds[++ncmd]= "PM=1"
}
cmds[++ncmd]= "MO=1"
if (elmoeth_multiquery(sl, ++ncmd, cmds, vals)==-1) {
print ELMO_ERR "failed to set user_mode to " mode
return (-1)
}
if (mode==5) {
ans= elmoeth_query(sl, "PA=DV[3]")
if (ans==-1) return (-1)
if (!ELMO_PAR[mne]["config"]) {
read_motors(0x6)
}
}
}
ans= elmoeth_query(sl, "UM")
if (sscanf(ans, "%d", mode) != 1) {
print ELMO_ERR "unable to get user_mode"
return (-1)
}
ELMO_PAR[mne]["mode"]= mode
return mode
}
#
# Return encoder reading
#
if (key == "enc_position") {
if (todo == "set") {
print ELMO_ERR "enc_position is read-only parameter"
}
ans= elmoeth_query(sl, "PX")
if (sscanf(ans, "%d", sta) != 1) {
print ELMO_ERR "unable to get enc_position"
return (-1)
}
return sta
}
#
# start a jog at defined speed or stop it (speed=0)
#
if (key == "jog_speed") {
if (todo == "set") {
if (p1 != 0) {
if (ELMO_PAR[mne]["jog"] != 0) {
print ELMO_ERR "jog motion already started"
} else {
val= motor_par(num, "sign") * p1 * motor_par(num, "step_size")
if (elmoeth_query(sl, sprintf("JV=%d", val))!=-1) {
ELMO_PAR[mne]["jog"]= p1
elmoeth__debug "start jog speed at " p1 " deg/sec [" val " steps/sec]"
elmoeth_query(sl, "BG")
}
}
} else {
# --- stop jog and wait deceleration
elmoeth_query(sl, "JV=0")
elmoeth_query(sl, "BG")
val= (ELMO_PAR[mne]["jog"]*motor_par(num, "step_size")) / \
motor_par(num, "velocity") * motor_par(num, "acceleration")
ELMO_PAR[mne]["jog"]= 0
elmoeth__debug "waiting deceleration " val " ms"
sleep(val/1000.)
val= elmoeth_query(sl, "MF")
if (val>0) {
print ELMO_ERR "motor failure on last jog command"
}
}
} else {
return ELMO_PAR[mne]["jog"]
}
}
#
# Jog speed range values
#
if (key == "jog_range") {
if (todo == "get") {
local val1 val2
val1= elmoeth_query(sl, "VL[2]") / motor_par(num, "step_size")
val2= elmoeth_query(sl, "VH[2]") / motor_par(num, "step_size")
return sprintf("%g:%g [deg/sec]", val1, val2)
} else {
print ELMO_ERR "jog_range is read-only"
}
}
#
# enable/disable slave control
#
if (key == "enable_slave") {
local val
if (motor_par(mne, "control_slave")!=1) {
print ELMO_ERR "Motor " mne " has no slave configured"
return (-1)
}
if (todo == "set") {
elmoeth_query(sl, sprintf("OB[1]=%d", p1==1?1:0))
}
val= elmoeth_query(sl, "OB[1]")
return (val==1)
}
}'
#%IU%
#%MDESC%
# MACRO MOTOR:
# Called by spec on motor operation.
#
def elmoeth_cmd(num,key,p1,p2) '{
local mne
local sl
local ans
local fnId ; fnId=sprintf("elmoeth_cmd([%s] [%s] [%s] [%s])", num, key, p1, p2)
if(num == "..") { return }
# get the motor mnemonic string
mne=motor_mne(num)
# Get the communication link
sl=motor_par(num,"address")
elmoeth__debug "entering " fnId
#
# return the current motor position in mm or deg
#
if (key == "position") {
local sta pos pos_str
if (ELMO_PAR[mne]["mode"] == 2) {
# --- in velocity mode, always return encoder position
pos= elmoeth_query(sl, "PX")
} else {
# --- in position mode, returns:
# encoder position if moving
# command position if motion finished
ans = elmoeth_query(sl,"MS")
if(sscanf(ans,"%d",sta) != 1) {
print ELMO_ERR "unable to get status"
return(0)
}
if (sta == 2) {
pos = elmoeth_query(sl, "PX")
} else {
# workaround for controller bug: when a limitswitch is hitten
# some times the PA position is wrong, only the encoder one
# can be trusted
#
# workaround for controller bug: on a ST command the PA remains
# to the target value and therefore is no more synchronized
# with the PX encoder value. Force PA to the PX value.
#
if(ELMO_PAR[mne]["abort_one"]) {
# give some time for the encoder stabilization
sleep(0.1)
# get encoder position
pos = elmoeth_query(sl, "PX")
elmoeth_query(sl,sprintf("PA=%d",pos))
# can not use SR bit28 which vanished
ELMO_PAR[mne]["abort_one"] = 0
} else {
# return target value (to avoid rounding effects for next motions??)
pos = elmoeth_query(sl, "PA")
}
}
}
pos_str = sprintf("%.15g", pos / motor_par(num,"step_size"))
return(pos_str)
}
#
# start a motion (p1==abs pos, p2==rel pos, with pos in mm or deg)
#
if (key == "start_one") {
local pos sta
if (ELMO_PAR[mne]["mode"] == 2) {
print ELMO_ERR "no motion allowed in jog mode"
return ".error."
}
# check first that the controller is ready to move
# bit0 of Status Register (page 3.135)
ans = elmoeth_query(sl,"SR")
if(sscanf(ans,"%d",sta) != 1) {
print ELMO_ERR "unable to get status"
return(-1)
}
if(sta&1) {
print ELMO_ERR "problem into the drive"
return ".error."
}
if(!(sta&(1<<4))) {
print ELMO_ERR "closed loop open"
return ".error."
}
# launch the absolute motion
pos=elmoeth_round(p1*motor_par(num,"step_size"))
elmoeth_query(sl,sprintf("PA=%d",pos))
elmoeth_query(sl,"BG")
return
}
#
# return the current motor status
#
if (key == "get_status") {
local _sta ret
local _ms _ls
if (ELMO_PAR[mne]["mode"] == 2) {
return MOT_OK
}
if (ELMO_PAR[mne]["homing"]) {
# --- homing case
ans= elmoeth_query(sl,"SR")
if (sscanf(ans,"%d",_sta) != 1) {
print ELMO_ERR "unable to get status"
return ".error."
}
_ms= _sta&(1<<7)
if (_ms) {
return MOT_MOVING
} else {
ELMO_PAR[mne]["homing"]= 0
ans= elmoeth_query(sl, "PA=DV[3]")
ret= MOT_OK
}
} else {
# --- moving case
ans= elmoeth_query(sl,"MS")
if (sscanf(ans, "%d", _sta) != 1) {
print ELMO_ERR "unable to get motion status"
return ".error."
}
if (_sta == 0) {
# --- target is reached
return MOT_OK
} else if (_sta == 1) {
# --- target not reached
ret= MOT_MOVING
} else if (_sta == 2) {
# --- still moving
return MOT_MOVING
} else if (_sta == 3) {
# --- motor is disabled
print ELMO_ERR "motor is disabled"
ret= MOT_FAULT
}
}
# --- check limits
ans= elmoeth_query(sl, "IP")
if (sscanf(ans, "%d", _ls) != 1) {
print ELMO_ERR "unable to read input port status"
return ".error."
}
if (motor_par(mne, "control_slave")==1) {
if (_ls & (1<<17)) {
print ELMO_ERR "Slave switch active"
}
}
if (_ls & (1<<6)) {
print ELMO_ERR "Forward Limit switch active"
return MOT_LIMP
} else if (_ls & (1<<7)) {
print ELMO_ERR "Reverse Limit switch active"
return MOT_LIMM
} else if (_ls & (1<<8)) {
print ELMO_ERR "Inhibit switch active"
return MOT_EMERGENCY
}
return (ret)
}
#
# stop a single motor
#
if (key == "abort_one") {
local nst sta pos rot cmd
elmoeth_query(sl,"ST")
ELMO_PAR[mne]["abort_one"]=1
# workaround for controller bug: a ST command while within
# an homing sequence lets the MS bit sets but still saying
# that the closed loop is on (MO==1). Unique solution found,
# cycle the closed loop to cleanup MS bits
if (ELMO_PAR[mne]["homing"]) {
elmoeth_par(num, "power", "set", 0)
sleep(0.1)
elmoeth_par(num, "power", "set", 1)
ELMO_PAR[mne]["homing"] = 0
}
sta= 1
nst= 0
while (sta && (nst<10)) {
sleep(MOTORSPOLLTIME)
ans= elmoeth_query(sl, "MS")
if (sscanf(ans, "%d", sta) != 1) {
print ELMO_ERR "unable to get status on abort"
return 0
}
nst+=1
}
return
}
#
# set position (p1=mm)
#
if (key == "set_position") {
local pos cmd
# The encoder can only be set if the motor is off
elmoeth_par(num,"power","set",0)
pos=elmoeth_round(p1*motor_par(num,"step_size"))
elmoeth_query(sl,sprintf("PX=%d",pos))
elmoeth_par(num,"power","set",1)
elmoeth_query(sl,sprintf("PA=%d",pos))
return
}
#
# set acceleration (p1=ms p2=steps/sec^2)
#
if (key == "acceleration") {
elmoeth_query(sl,sprintf("AC=%d",p2))
elmoeth_query(sl,sprintf("DC=%d",p2))
return
}
#
# set base_rate (p1=rate in Hz)
#
if (key == "base_rate") {
return
}
#
# set slew_rate (p1=rate in Hz)
#
if (key == "slew_rate") {
elmoeth_query(sl,sprintf("SP=%d",p1))
return
}
#
# home search
#
if (key == "search") {
if (substr(p1,1,4) == "home") {
local pos dir icmd ncmd cmds[] vals[]
if (p1=="home-") {
dir= -1
} else {
dir= 1
}
pos= motor_par(num, "home_position")
_elmoeth_start_home(num, dir, 1, pos)
} else {
print ELMO_ERR "Only home search implemented"
return ".error."
}
}
}'
def _elmoeth_start_home(mnum, dir, setpos, dialpos) '{
local sl pos
local ncmd cmds[] vals[]
sl= motor_par(mnum, "address")
if (setpos) {
pos= elmoeth_round(pos*motor_par(mnum, "sign")*motor_par(mnum, "step_size"))
ncmd= 0
cmds[ncmd++]= "HM[3]=3"
cmds[ncmd++]= sprintf("HM[2]=%d", pos)
cmds[ncmd++]= "HM[4]=0"
cmds[ncmd++]= "HM[5]=0"
cmds[ncmd++]= "HM[1]=1"
} else {
ncmd= 0
cmds[ncmd++]= "HM[3]=3"
cmds[ncmd++]= "HM[4]=0"
cmds[ncmd++]= "HM[5]=2"
cmds[ncmd++]= "HM[1]=1"
}
if (elmoeth_multiquery(sl, ncmd, cmds, vals)==-1) {
print ELMO_ERR "failed to setup HOME search"
return ".error."
}
ncmd= 0
cmds[ncmd++]= sprintf("JV=%d", dir*motor_par(mnum, "sign")*motor_par(mnum, "velocity"))
cmds[ncmd++]= "BG"
if (elmoeth_multiquery(sl, ncmd, cmds, vals)==-1) {
print ELMO_ERR "failed to start HOME search"
return ".error."
}
ELMO_PAR[mne]["homing"]= 1
}'
#%IU%(serialline, command)
#%MDESC%
# Send the command to the ELMO controller and returns its string
# answer if any or empty string if none.
# If case of error, returns -1.
#
def elmoeth_query(sl,cmd) '{
local cmd_send ans ret
elmoeth__debug "sending " cmd
cmd_send= cmd "\r"
# Yes, a little bit paranoid
sock_par(sl, "flush")
# Chaud devant
sock_put(sl, cmd_send)
# The controller always echo the command received
ans= sock_get(sl,";")
ret= elmoeth_parseanswer(cmd, ans)
elmoeth__debug "returning " ret
return(ret)
}'
#%IU%(command, answer)
#%MDESC%
# Parse controller serial response. Check command echo, error character,
# and return answer or (-1) on error
#
def elmoeth_parseanswer(cmd, ans) '{
local cmd_term cmd_echo ret
if (ans == "") {
print ELMO_ERR "missing command echo on", cmd
return (-1)
}
cmd_term= index(ans, "\r")
cmd_echo= substr(ans, 1, cmd_term-1)
if (cmd_echo != cmd) {
print ELMO_ERR "wrong command echo on", cmd
return (-1)
}
ret= substr(ans, cmd_term+1, length(ans)-cmd_term-1)
if (substr(ret, length(ret), 1) == "?") {
print ELMO_ERR "when sending", cmd
elmoeth_printerr(substr(ret, length(ret)-1, 1))
return (-1)
}
if (!length(ret)) {
ret= 0
}
return (ret)
}'
#%IU%
#%MDESC%
# Send multiple commands to controller (ncmd commands defined in ass array cmds[]).
# Parse answer of each commands and return answer or (-1) on errors in ass array vals[]
# Global return is 0 if sucess, -1 if one of the commands fails.
#
def elmoeth_multiquery(sl, ncmd, cmds, vals) '{
local icmd ans ret
ret= 0
sock_par(sl, "flush")
for (icmd=0; icmd<ncmd; icmd++) {
elmoeth__debug "sending " cmds[icmd]
sock_put(sl, sprintf("%s\r", cmds[icmd]))
ans= sock_get(sl)
vals[icmd]= elmoeth_parseanswer(cmds[icmd], ans)
elmoeth__debug cmds[icmd] " returns " vals[icmd]
if (vals[icmd]==-1) {
ret= -1
}
}
return (ret)
}'
#%IU%(err)
#%MDESC%
# Print a string message corresponding to the binary error code given.
# See "Command Reference Manual" page 69 Command EC for detailed list.
#
def elmoeth_printerr(err_ch) '{
local ELMOETH_ERR
local err
err=asc(err_ch)
ELMOETH_ERR[2]= "Bad command"
ELMOETH_ERR[3]= "Bad index for array"
ELMOETH_ERR[16]= "Array is expected"
ELMOETH_ERR[19]= "Wrong command syntax"
ELMOETH_ERR[21]= "Operand out of range"
ELMOETH_ERR[23]= "Command cannot be assigned"
ELMOETH_ERR[28]= "Out of Limit range"
ELMOETH_ERR[48]= "Motor could not start. See CD"
ELMOETH_ERR[51]= "Inhibit OR Aborts input are active"
ELMOETH_ERR[57]= "Motor musst be OFF"
ELMOETH_ERR[58]= "Motor musst be ON"
ELMOETH_ERR[60]= "Bad Unit Mode"
ELMOETH_ERR[168]= "Speed too large to start motor"
ELMOETH_ERR[204]= "External inhibit input detected"
ELMOETH_ERR[207]= "Speed error limit exceeded"
ELMOETH_ERR[208]= "Position error limit"
ELMOETH_ERR[233]= "Under voltage protection"
ELMOETH_ERR[235]= "Over voltage protection"
ELMOETH_ERR[237]= "Safety switch"
ELMOETH_ERR[243]= "Over temperature protection"
if (length(ELMOETH_ERR[err])>0) {
print ELMO_CTLERR ELMOETH_ERR[err]
} else {
print ELMO_CTLERR "Not listed error. Refer Command Reference Manual (p.69)"
}
}'
#%UU% <motor> [<user_position>]
#%MDESC%
# Search home switch in positive direction. At home switch,
# controller sets the dial position to spec "home_position".
# If asked, change offset according to given <user_position>
#
def elmoethhome '{
local mnum pos sl
if (($#!=1)&&($#!=2)) {
print "Usage: $0 <motor> [<user_position>]"
exit
}
mnum= motor_num($1)
if (motor_par(mnum, "device_id")!="elmoeth") {
print $1 " is not controlled by elmoeth !!"
exit
}
# --- launch and wait homing
print "Starting homing procedure ..."
chg_dial(mnum, "home+")
sleep(0.1)
move_poll
# --- set command pos to encoder one
sl = motor_par(mnum, "address")
pos = elmoeth_query(sl, "PX")
elmoeth_query(sl,sprintf("PA=%d",pos))
read_motors(0x6)
print "Homing finished."
# --- read captured home pos
homepos = elmoeth_query(sl, "HM[7]")
print "Captured encoder steps at home =", homepos
print "Captured dial position at home =", homepos / motor_par(rot, "step_size")
print "Set dial position at home =", motor_par(rot, "home_position")
print
# --- report dial position
get_angles
print "Current Dial Position =", dial(mnum, A[mnum])
# --- report user position
if ($# == 2) {
chg_offset(mnum, $2)
get_angles
}
print "Current User Position =", A[mnum]
}'
#%IU%(x)
#%MDESC%
# Return a well rounded integer as opposed to int() which always
# round down
#
def elmoeth_round(x) '{ if (x>=0) return int(x+0.5); else return int(x-0.5) }'
#%UU% <motor>
#%MDESC%
# Report detailed status and input port states
#
def elmoethstatus '{
local err prefix val val1 val2 val3
local ans status sl
if ($# != 1) {
print "Usage: $0 <motor>"
exit
}
num= motor_num("$1")
if (num==-1) {
print "Wrong motor name"
exit
}
sl= motor_par(num, "address")
ans= elmoeth_query(sl, "SR")
if (sscanf(ans, "%d", status)!=1) {
print ELMO_ERR "Failed to read status"
exit
}
printf("\nSTATUS REGISTER:\n")
printf("SR> ---- [0x%08x]\n", status)
val = status & ( (1<<0) | (1<<1) | (1<<2) | (1<<3))
prefix = "AMPLIFIER"
if(val == 0) {
printf("--- %s: %s\n", prefix, "OK") }
else if(val == 3) {
printf("--- %s: %s\n", prefix, "UNDER voltage") }
else if(val == 5) {
printf("--- %s: %s\n", prefix, "OVER voltage") }
else if(val == 7) {
printf("--- %s: %s\n", prefix, "SAFETY alarm")
if((status & (1<<14)) == 0) {
printf(" --- %s\n", "SAFETY Input 1 alarm") }
if((status & (1<<15)) == 0) {
printf(" --- %s\n", "SAFETY Input 2 alarm") } }
else if(val == 11) {
printf("--- %s: %s\n", prefix, "SHORT protection - over current") }
else if(val == 13) {
printf("--- %s: %s\n", prefix, "OVER temperature") }
else if(val == 15) {
printf("--- %s: %s\n", prefix, "Aditional ABORT activated") }
else {
printf("--- %s: %s [%d]\n", prefix, "unknow value", val) }
val = status & (1<<4)
printf("--- %s: %s\n","SERVO", val ? "enabled" : "disabled")
val = status & (1<<5)
printf("--- %s: %s\n","Ext REFERENCE", val ? "enabled" : "disabled")
val = status & (1<<6)
printf("--- %s: %s\n","FAULT", val ? "OCURRED" : "none")
val = status & (1<<7)
printf("--- %s: %s\n","HOMING", val ? "activated" : "none")
val = (status & ((1<<8) | (1<<9) | (1<<10) | (1<<11))) >> 8
prefix = "PROFILE MODE"
if(val == 0) {
printf("--- %s: %s\n", prefix, "none") }
else if(val == 1) {
printf("--- %s: %s\n", prefix, "position") }
else if(val == 3) {
printf("--- %s: %s\n", prefix, "velocity") }
else if(val == 4) {
printf("--- %s: %s\n", prefix, "torque") }
else if(val == 6) {
printf("--- %s: %s\n", prefix, "homing") }
else if(val == 7) {
printf("--- %s: %s\n", prefix, "interpolate position") }
else if(val == 8) {
printf("--- %s: %s\n", prefix, "cyclic sync position") }
else if(val == 9) {
printf("--- %s: %s\n", prefix, "cyclic sync velocity") }
else if(val == 10) {
printf("--- %s: %s\n", prefix, "cyclic sync torque") }
else {
printf("--- %s: %s [%d]\n", prefix, "unknow value", val) }
val = status & (1<<12)
printf("--- %s: %s\n","USER PROGRAM", val ? "running" : "NOT running")
val = status & (1<<13)
printf("--- %s: %s\n","CURRRENT limit", val ? "limited to CL[1]" : "none")
val = status & (1<<14)
printf("--- %s: %s\n","SAFETY INPUT 1", val ? "OK (=1)" : "ALARM (=0)")
val = status & (1<<15)
printf("--- %s: %s\n","SAFETY INPUT 2", val ? "OK (=1)" : "ALARM (=0)" )
val = (status & ((1<<16) | (1<<17) )) >> 16
prefix = "RECORDER status"
if(val == 0) {
printf("--- %s: %s\n", prefix, "not active") }
else if(val == 1) {
printf("--- %s: %s\n", prefix, "waiting for the trigger") }
else if(val == 2) {
printf("--- %s: %s\n", prefix, "completed task") }
else if(val == 3) {
printf("--- %s: %s\n", prefix, "active") }
val = status & (1<<18)
printf("--- %s: %s\n","TARGET", val ? "reached" : "NOT reached" )
val = status & (1<<21)
printf("--- %s: %s\n","SHUNT", val ? "OFF" : "ON")
val = status & (1<<22)
printf("--- %s: %s\n","MOTOR", val ? "ON" : "OFF" )
val = status & (1<<23)
printf("--- %s: %s\n","SPEED", val ? "!= 0" : "== 0" )
val1 = (status & (1<<24)) != 0
val2 = (status & (1<<25)) != 0
val3 = (status & (1<<26)) != 0
printf("--- %s: %d %d %d\n","HALL A, B, C state", val1, val2, val3 )
val = status & (1<<27)
printf("--- %s: %s\n","SAFE TORQUE", val ? "FAILED" : "none" )
val = status & (1<<28)
printf("--- %s: %s\n","PROFILE stop", val ? "STOPPED" : "none" )
val = status & (1<<30)
printf("--- %s: %s\n","PTP buffer", val ? "FULL" : "OK")
ans= elmoeth_query(sl, "IP")
if (sscanf(ans, "%d", status)!=1) {
print ELMO_ERR "Failed to read status"
exit
}
printf("\nINPUT PORT REGISTER:\n")
printf("IP> ---- [0x%08x]\n", status)
local IP_MSG[]
IP_MSG[0]= "General purpose input"
IP_MSG[1]= "Safety"
IP_MSG[2]= "Main home switch"
IP_MSG[3]= "Aux. home switch"
IP_MSG[4]= "Soft stop"
IP_MSG[5]= "Hard stop"
IP_MSG[6]= "Forward limit switch"
IP_MSG[7]= "Reverse limit switch"
IP_MSG[8]= "Inhibit switch"
IP_MSG[9]= "Hardware motion begin"
IP_MSG[10]= "Abort function"
IP_MSG[16]= "Digital input 1"
IP_MSG[17]= "Digital input 2"
IP_MSG[18]= "Digital input 3"
IP_MSG[19]= "Digital input 4"
IP_MSG[20]= "Digital input 5"
IP_MSG[21]= "Digital input 6"
for (ip=0; ip<11; ip++) {
val= (status & (1<<ip)) ? "ON" : "OFF"
printf("--- %28.28s : %s\n", IP_MSG[ip], val)
}
for (ip=16; ip<22; ip++) {
val= (status & (1<<ip)) ? "ON" : "OFF"
printf("--- %28.28s : %s\n", IP_MSG[ip], val)
}
}'
#%MACROS%
#%IMACROS%
#%AUTHOR% MP BLISS (Original 8/2011).
# %BR%$Revision: 1.2 $ / $Date: 2017/06/13 15:11:53 $
#%TOC%
|
