#%TITLE% ELMO.MAC
#
#%NAME%
# Macros to control ELMO motor controller through macro motor.
# Tested only with model "Whistle 2.02.07.00" through RS232 and
# 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 "elmo"
# %DT% -
# The "ADDR" must be set to the serial line already declared
# in the current session config (ex: "6"). The serial line
# settings are %B%19200%B% bauds with mode %B%Raw%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% -
# The "Linear/Rotary" field set to 1 for rotary motors, 0 otherwise
# %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% closed_loop
# %DD% Switch on(1) / off(0) the closed loop
# %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.
# %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 19200
constant ELMO_ERR "ELMO ERROR: "
constant ELMO_CTLERR "ELMO CONTROLLER ERROR: "
#%UU%
#%MDESC%
# Toggle on/off debug messages
#
def elmodebug '{
global ELMO_DEBUG
if (ELMO_DEBUG) {
rdef elmo__debug \'#\$#\'
print "ELMO debug mode is OFF"
ELMO_DEBUG=0
} else {
rdef elmo__debug \'print "ELMO: "\'
print "ELMO debug mode is ON"
ELMO_DEBUG=1
}
}'
def elmodebug_init '{
if(!(whatis("elmo__debug")&0x2)) {
global ELMO_DEBUG
rdef elmo__debug \'#\$#\'
ELMO_DEBUG= 0
}
}'
elmodebug_init
#%IU%
#%MDESC%
# MACRO MOTOR:
# Called by spec after reading the config file
#
def elmo_config(num,type,p1,p2,p3) '{
local sl ans mne rot um
global ELMO_PAR[]
local i
# Get the communication link
sl=elmo_ADDR
# p1==controller index p2==number of motors supported
if(type=="ctrl") {
# Check serial line validity
if(ser_par(sl,"device_id")==-1) {
print ELMO_ERR "wrong serial line"
return ".error."
}
# Check serial line settings
# NOTE MP 002Aug2011: seems to not work with TACO serial line
#if(ser_par(sl,"baud") != ELMO_SLBAUD) {
# print ELMO_ERR "wrong serial line baud rate, must be: "ELMO_SLBAUD
# return ".error."
#}
# Check that the controller is alive
ans = elmo_query(sl,"VR")
if((ans=="") || (ans==-1)) {
print ELMO_ERR "missing controller"
return ".error."
}
print "ELMO: new controller: "ans
}
# p1==unit p2==module p3==channel
if(type=="mot") {
mne= motor_mne(num)
ELMO_PAR[mne]["config"]= 1
rot= motor_par(num, "rotary")
# user mode = 4 : linear ; 5 : rotation
um= rot ? 5 : 4
# Check user-mode
mode= elmo_par(num, "user_mode", "get")
if (mode != um) {
mode= elmo_par(num, "user_mode", "set", um)
if (mode != um) {
ELMO_PAR[mne]["config"]= 0
return ".error."
}
} else {
# Check closed loop on
power= elmo_par(num, "power", "get")
if (power==0) {
power= elmo_par(num, "power", "set", 1)
if (power!=1) {
ELMO_PAR[mne]["config"]= 0
return ".error."
}
}
}
# reset jog speed
ELMO_PAR[mne]["jog"]= 0
ELMO_PAR[mne]["config"]= 0
print "ELMO: closed loop switched on for motor: \""mne"\""
}
}'
#%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 elmo_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,closed_loop,user_mode,enc_position,jog_speed,jog_range")
}
#
#
#
if (key == "closed_loop") {
local sta ans
# Change the closed loop state
if(todo == "set") {
elmo_query(sl,sprintf("MO=%d",(p1==1?1:0)))
}
# Return the current state of closed loop
# bit4 of Status Register (page 3.135)
ans = elmo_query(sl,"SR")
if(sscanf(ans,"%d",sta) != 1) {
print ELMO_ERR "unable to get status"
return(-1)
}
return((sta&(1<<4))?1:0)
}
if (key == "power") {
local sta ans
if (todo == "set") {
elmo_query(sl, sprintf("MO=%d", (p1==1?1:0)))
}
ans= elmo_query(sl, "MO")
if (sscanf(ans, "%d", sta) != 1) {
print ELMO_ERR "unable to get power status"
return (-1)
}
if (ELMO_PAR["srot"]["mode"] != 2) {
if ((todo == "set")&&(sta==1)) {
elmo_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 (elmo_multiquery(sl, ++ncmd, cmds, vals)==-1) {
print ELMO_ERR "failed to set user_mode to " mode
return (-1)
}
if ((mode==4) || (mode==5)) {
ans= elmo_query(sl, "PA=DV[3]")
if (ans==-1) return (-1)
if (!ELMO_PAR[mne]["config"]) {
read_motors(0x6)
}
}
}
ans= elmo_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
}
if (key == "enc_position") {
if (todo == "set") {
print ELMO_ERR "enc_position is read-only parameter"
}
rot= motor_par(num, "rotary")
cmd= rot ? "PX" : "PY"
ans= elmo_query(sl, cmd)
if (sscanf(ans, "%d", sta) != 1) {
print ELMO_ERR "unable to get enc_position"
return (-1)
}
return sta
}
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 (elmo_query(sl, sprintf("JV=%d", val))!=-1) {
ELMO_PAR[mne]["jog"]= p1
elmo__debug "start jog speed at " p1 " deg/sec [" val " steps/sec]"
elmo_query(sl, "BG")
}
}
} else {
# --- stop jog and wait deceleration
elmo_query(sl, "JV=0")
elmo_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
elmo__debug "waiting deceleration " val " ms"
sleep(val/1000.)
val= elmo_query(sl, "MF")
if (val>0) {
print ELMO_ERR "motor failure on last jog command"
}
}
} else {
return ELMO_PAR[mne]["jog"]
}
}
if (key == "jog_range") {
if (todo == "get") {
local val1 val2
val1= elmo_query(sl, "VL[2]") / motor_par(num, "step_size")
val2= elmo_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"
}
}
}'
#%IU%
#%MDESC%
# MACRO MOTOR:
# Called by spec on motor operation.
#
def elmo_cmd(num,key,p1,p2) '{
local mne
local sl
local ans
if(num == "..") { return }
# get the motor mnemonic string
mne=motor_mne(num)
# Get the communication link
sl=motor_par(num,"address")
#
# return the current motor position in mm or deg
#
if (key == "position") {
local sta pos pos_cmd pos_str
pos_cmd= motor_par(num, "rotary") ? "PX" : "PY"
if (ELMO_PAR[mne]["mode"] == 2) {
# --- in velocity mode, always return encoder position
pos= elmo_query(sl, pos_cmd)
} else {
# --- in position mode, returns:
# encoder position if moving
# command position if motion finished
ans = elmo_query(sl,"MS")
if(sscanf(ans,"%d",sta) != 1) {
print ELMO_ERR "unable to get status"
return(0)
}
if (sta == 2) {
pos = elmo_query(sl, pos_cmd)
} 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 = elmo_query(sl, pos_cmd)
elmo_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 = elmo_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 = elmo_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=elmo_round(p1*motor_par(num,"step_size"))
elmo_query(sl,sprintf("PA=%d",pos))
elmo_query(sl,"BG")
return
}
#
# return the current motor status
#
if (key == "get_status") {
local ret
local sta
local _ms _ls _lh
if (ELMO_PAR[mne]["mode"] == 2) {
return 0
}
ret = 0
ans = elmo_query(sl,"SR")
if(sscanf(ans,"%d",sta) != 1) {
print ELMO_ERR "unable to get status. Will try again ..."
sleep(MOTORSPOLLTIME)
ans = elmo_query(sl, "SR")
if (sscanf(ans, "%d", sta) != 1) {
print ELMO_ERR "failed to get status"
return(0)
}
}
# motor is moving
if (ELMO_PAR[mne]["homing"]) {
_ms= (sta >> 11) & 0x1
if (_ms == 0) {
ELMO_PAR[mne]["homing"]= 0
ans= elmo_query(sl, "PA=DV[3]")
}
} else {
_ms= (sta >> 14) & 0x3
}
if (_ms != 0) {
ret = 0x02
}
if(ELMO_DEBUG) {
printf("SR: 0x%08x\n",elmo_query(sl, "SR"))
printf("IP: 0x%08x\n",elmo_query(sl, "IP"))
}
# limit switch active
_ls= (sta >> 28) & 0x1
if (_ls) {
_lh= elmo_query(sl, "IP")
_lh= (_lh >> 10) & 0x3
if (_lh&0x1) {
ret = 0x08
} else if (_lh&0x2) {
ret = 0x04
}
# the controller has already stopped the motion
elmo_cmd(num, "abort_one")
}
elmo__debug "get_status " ret
return(ret)
}
#
# stop a single motor
#
if (key == "abort_one") {
local nst sta pos rot cmd
elmo_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"]) {
elmo_par(num, "power", "set", 0)
sleep(0.1)
elmo_par(num, "power", "set", 1)
ELMO_PAR[mne]["homing"] = 0
}
sta= 1
nst= 0
while (sta && (nst<10)) {
sleep(MOTORSPOLLTIME)
ans= elmo_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 rot cmd
rot= motor_par(num, "rotary")
cmd= rot ? "PX" : "PY"
# The encoder can only be set if the motor is off
elmo_par(num,"closed_loop","set",0)
pos=elmo_round(p1*motor_par(num,"step_size"))
elmo_query(sl,sprintf("%s=%d",cmd,pos))
elmo_par(num,"closed_loop","set",1)
elmo_query(sl,sprintf("PA=%d",pos))
return
}
#
# set acceleration (p1=ms p2=steps/sec^2)
#
if (key == "acceleration") {
elmo_query(sl,sprintf("AC=%d",p2))
elmo_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") {
elmo_query(sl,sprintf("SP=%d",p1))
return
}
#
# home search
#
if (key == "search") {
if (substr(p1,1,4) == "home") {
local pos dir ncmd cmds[] vals[]
if (p1=="home-") {
dir= -1
} else {
dir= 1
}
pos= motor_par(num, "home_position")
pos= elmo_round(pos*motor_par(num, "sign")*motor_par(num, "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"
if (elmo_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(num, "sign")*motor_par(num, "velocity"))
cmds[ncmd++]= "BG"
if (elmo_multiquery(sl, ncmd, cmds, vals)==-1) {
print ELMO_ERR "failed to start HOME search"
return ".error."
}
ELMO_PAR[mne]["homing"]= 1
} else {
print ELMO_ERR "Only home search implemented"
return ".error."
}
}
}'
#%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 elmo_query(sl,cmd) '{
local cmd_send ans ret ntry
elmo__debug "sending " cmd
cmd_send= cmd "\r"
ntry= 0
ret= -1
while ((ret == -1) && (ntry < 3)) {
ntry+=1
# Yes, a little bit paranoid
ser_par(sl, "flush")
# Chaud devant
ser_put(sl, cmd_send)
# The controller always echo the command received
ans = ser_get(sl,";")
ret= elmo_parseanswer(cmd, ans)
if (ret == -1) {
if (ntry < 3) {
elmo__debug "query failed, retry #" ntry
} else {
elmo__debug "query still failed, give up."
}
}
}
elmo__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 elmo_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
elmo_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 elmo_multiquery(sl, ncmd, cmds, vals) '{
local cmd_send icmd ans ret
cmd_send= ""
for (icmd=0; icmd<ncmd; icmd++) {
cmd_send= cmd_send sprintf("%s\r", cmds[icmd])
elmo__debug "sending " cmds[icmd]
}
ser_par(sl, "flush")
ser_put(sl, cmd_send)
ret= 0
for (icmd=0; icmd<ncmd; icmd++) {
ans= ser_get(sl, ";")
vals[icmd]= elmo_parseanswer(cmds[icmd], ans)
elmo__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 3-32 for detailed list.
#
def elmo_printerr(err_ch) '{
local err
err=asc(err_ch)
if(err==2) {
print ELMO_CTLERR "bad command"
} else if(err==12) {
print ELMO_CTLERR "command not available in this unit mode"
} else if(err==18) {
print ELMO_CTLERR "empty assign"
} else if(err==21) {
print ELMO_CTLERR "operand out of range"
} else if(err==25) {
print ELMO_CTLERR "command not valid while moving"
} else if(err==28) {
print ELMO_CTLERR "out of limit range"
} else if(err==57) {
print ELMO_CTLERR "motor must be off"
} else if(err==58) {
print ELMO_CTLERR "motor must be on"
} else if(err==66) {
print ELMO_CTLERR "drive not ready"
} else if(err==84) {
print ELMO_CTLERR "the system is not in point to point mode"
} else if(err==148) {
print ELMO_CTLERR "nothing in the expression"
} else if(err==168) {
print ELMO_CTLERR "speed to large to start the motor"
} else {
print ELMO_CTLERR "unknown error (check MAN-SIMCR.pdf page 3-32): "err
}
}'
#%UU% <motor> <user_position>
#%MDESC%
# Search home switch in positive direction,
# sets the position to 0 and then move motor to user_position
#
def elmohome '{
if ($#!=2) {
print "Usage: $0 <motor> <user_position>"
exit
}
if (motor_par($1, "device_id")!="elmo") {
print $1 " is not controlled by elmo !!"
exit
}
print "Starting homing procedure"
chg_dial($1, "home+")
move_poll
print "Moving to user position"
umv $1 $2
}'
#%IU%(x)
#%MDESC%
# Return a well rounded integer as opposed to int() which always
# round down
#
def elmo_round(x) '{ if (x>=0) return int(x+0.5); else return int(x-0.5) }'
#%MACROS%
#%IMACROS%
#%AUTHOR% MP BLISS (Original 8/2011).
# %BR%$Revision: 1.13 $ / $Date: 2017/06/13 15:03:55 $
#%TOC%
|
