#%TITLE% FLEXDC.MAC
#
#%NAME%
# Macro motors for FlexDc controller from Nanomotion
#
#%CATEGORY% Positioning, Isg
#
#%DESCRIPTION%
#%DL%
#%DT%Configuring macro motors %DD%
# %DL%
# %DT% 1)
# You must have an entry in "SERIAL" table for each controller
# %DL%
# %DT% -
# The "DEVICE" and "TYPE" fields depends on the SL used
# %DT% -
# The "BAUD" field must be set "38400"
# %DT% -
# The "MODE" field must be set "raw"
# %XDL%
#
# %DT% 2)
# You must have an entry in "MOTORS" table for each controller
# %DL%
# %DT% -
# The "DEVICE" field must be set to string "flexdc"
# %DT% -
# The "ADDR" must be set to the serial line entry in the SERIAL
# field in the config.
# %DT% -
# The "NUM" should be set to 2
# %DT% -
# The "TYPE" field must be set to "Macro Motors"
# %XDL%
# %DT% 3)
# 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 "Chan" field, numbered from 0,
# must be set to the channel on the FLEXDC controller
# %XDL%
# %XDL%
#
#%XDL%
#
#%END%
#
need spec_utils
constant FLEXDC_TAB "FLEXDC"
constant FLEXDC_EOC "\r"
constant FLEXDC_PPT ">"
######################################################################
############################ ###########################
############################ MACRO MOTOR ###########################
############################ ###########################
######################################################################
#%IU%
#%MDESC%
# MACRO MOTOR:
# Called by spec after reading the config file
#
def flexdc_config(num, type, p1, p2, p3) '{
local dev
local ans
# p1==?? p2==number of motors supported
if(type=="ctrl") {
# get the serial line to talk to the controller
dev = flexdc_ADDR
# check if the serial line is valid
if(ser_par(dev, "device_id") == -1) {
printf("%s ERROR: missing ADDR field or bad one, no SL\n", FLEXDC_TAB)
return(".error.")
}
# check if the controller is there,
# disable all the associated motors otherwise
# get controller firmware revision
ans = _flexdc_query(dev, "XVR")
if(ans == "") {
printf("%s ERROR: missing answer from controller\n", FLEXDC_TAB)
return(".error.")
}
printf("%s: found controller: %s\n", FLEXDC_TAB, ans)
# normal end
return(0)
}
# p1==unit p2==module p3==channel
if(type=="mot") {
local chan
local cmd
local mne
# get the serial line to talk to the controller
dev = motor_par(num, "address")
mne = motor_name(num)
# get the channel
if((p3<0) || (p3>1)) {
printf("%s ERROR: invalid channel\n", FLEXDC_TAB)
return(".error.")
}
chan = (p3==0)?"X":"Y"
# enabling the servo loop, motor is on
printf("%s: enabling servo loop on channel %s (motor \"%s\")\n", \
FLEXDC_TAB, chan, mne)
cmd=sprintf("%sMO=1", chan)
if(_flexdc_wr(dev, cmd)) {
printf("%s ERROR: enabling servo loop\n", FLEXDC_TAB)
return(".error.")
}
# set "point to point" motion mode
cmd=sprintf("%sMM=0", chan)
if(_flexdc_wr(dev, cmd)) {
printf("%s ERROR: setting \"point to point\" motion mode\n", FLEXDC_TAB)
return(".error.")
}
cmd=sprintf("%sSM=0", chan)
if(_flexdc_wr(dev, cmd)) {
printf("%s ERROR: setting \"point to point\" motion mode\n", FLEXDC_TAB)
return(".error.")
}
# define no smoothing
cmd=sprintf("%sWW=0", chan)
if(_flexdc_wr(dev, cmd)) {
printf("%s ERROR: setting no smoothing mode\n", FLEXDC_TAB)
return(".error.")
}
}
}'
#%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 flexdc_par(num, key, todo, p1) '{
local dev
local mne
local chan
# nothing to do for the controller
if(num == "..") {
return(0)
}
# get the serial to talk to the controller
dev = motor_par(num, "address")
mne = motor_name(num)
# get the channel
chan = (motor_par(num, "channel")==0)?"X":"Y"
#
# set the dead band of controller servo loop (p1=steps)
#
if (key == "dc_dead_band") {
local val
# activate the dead zone
# get the actual motor position
ans = _flexdc_query(dev, sprintf("%sCA[33]", chan))
if(ans == "") {
printf("%s ERROR: reading dead zone config\n", FLEXDC_TAB)
return(".error.")
}
# get the position in steps
if(sscanf(ans, "%d", val) != 1) {
printf("%s ERROR: parsing dead zone config\n", FLEXDC_TAB)
return(".error.")
}
# set the dead zone min value
cmd=sprintf("%sCA[33]=%d", chan, (val|1))
if(_flexdc_wr(dev, cmd)) {
printf("%s ERROR: setting dead zone config\n", FLEXDC_TAB)
return(".error.")
}
# set the dead zone min value
cmd=sprintf("%sCA[36]=%d", chan, p1)
if(_flexdc_wr(dev, cmd)) {
printf("%s ERROR: setting max dead zone\n", FLEXDC_TAB)
return(".error.")
}
# set the dead zone max value
cmd=sprintf("%sCA[37]=%d", chan, p1)
if(_flexdc_wr(dev, cmd)) {
printf("%s ERROR: setting max dead zone\n", FLEXDC_TAB)
return(".error.")
}
}
}'
#%IU%
#%MDESC%
# MACRO MOTOR: Called by spec on motor operation.
def flexdc_cmd(num, key, p1, p2) '{
local dev
local mne
local chan
# nothing to do for the controller
if(num == "..") {
return(0)
}
# get the serial to talk to the controller
dev = motor_par(num, "address")
mne = motor_name(num)
# get the channel
chan = (motor_par(num, "channel")==0)?"X":"Y"
#
# return the current motor position in mm or deg
#
if (key == "position") {
local pos_str
local pos
# get the actual motor position
ans = _flexdc_query(dev, sprintf("%sPS", chan))
if(ans == "") {
printf("%s ERROR: reading motor position\n", FLEXDC_TAB)
return(".error.")
}
# get the position in steps
if(sscanf(ans, "%f", pos) != 1) {
printf("%s ERROR: parsing motor position\n", FLEXDC_TAB)
return(".error.")
}
# convert the positinin mm
pos_str = sprintf("%.15g", pos / motor_par(num, "step_size"))
# normal end
return(pos_str)
}
#
# start a motion (p1==abs pos, p2==rel pos, with pos in mm or deg)
#
if (key == "start_one") {
local pos
# get the absolute position in steps
pos = round_nearest(p1*motor_par(num, "step_size"))
# prepare an absolution motion
if(_flexdc_wr(dev, sprintf("%sAP=%d", chan, pos))) {
printf("%s ERROR: preparing the motion\n", FLEXDC_TAB)
return(".error.")
}
# launch the motion
if(_flexdc_wr(dev, sprintf("%sBG", chan))) {
printf("%s ERROR: launching the motion\n", FLEXDC_TAB)
return(".error.")
}
}
#
# return the current motor status
#
if (key == "get_status") {
local ans
local ret
local sta
ret = 0
sta = 0
# get the current axis status
ans = _flexdc_query(dev, sprintf("%sMS", chan))
if(ans == "") {
printf("%s ERROR: reading axis status\n", FLEXDC_TAB)
return(".error.")
}
# get the bitmask from the answer
if(sscanf(ans, "%x", sta) != 1)
{
printf("%s ERROR: unable to get status for: \"%s\"\n", FLEXDC_TAB, mne)
return(".error.")
}
# get moving status
#if(sta & (1<<0)) { ret |= 0x02; } else {sleep(.2) }
if(sta & (1<<0)) ret |= 0x02;
# get the current axis status
ans = _flexdc_query(dev, sprintf("%sEM", chan))
if(ans == "") {
printf("%s ERROR: reading axis status\n", FLEXDC_TAB)
return(".error.")
}
# get the bitmask from the answer
if(sscanf(ans, "%x", sta) != 1)
{
printf("%s ERROR: unable to get status for: \"%s\"\n", FLEXDC_TAB, mne)
return(".error.")
}
# get limitswitches status (RLS)
if(sta & (1<<3)) ret |= 0x04;
# get limitswitches status (FLS)
if(sta & (1<<2)) ret |= 0x08;
# normal end
return(ret)
}
#
# stop a single motor
#
if (key == "abort_one") {
# Stop motion (with deceleration)
# For an abort (without deceleration) use rather the command "AB"
if(_flexdc_wr(dev, sprintf("%sST", chan))) {
printf("%s ERROR: stopping motion\n", FLEXDC_TAB)
return(".error.")
}
}
#
# set position (p1=mm)
#
if (key == "set_position") {
local pos
# get the absolute position in steps
pos = round_nearest(p1*motor_par(num, "step_size"))
# prepare an absolution motion
if(_flexdc_wr(dev, sprintf("%sPS=%d", chan, pos))) {
printf("%s ERROR: setting motor position\n", FLEXDC_TAB)
return(".error.")
}
}
#
# set the velocity (p1=steps/sec)
#
if (key == "slew_rate") {
# set the acceleration in steps/sec
if(_flexdc_wr(dev, sprintf("%sSP=%d", chan, p1))) {
printf("%s ERROR: setting velecoty\n", FLEXDC_TAB)
return(".error.")
}
}
#
# set the acceleration (p1=mS p2=steps/sec^2
#
if (key == "acceleration") {
# set the acceleration in steps/sec^2
if(_flexdc_wr(dev, sprintf("%sAC=%d", chan, p2))) {
printf("%s ERROR: setting acceleration\n", FLEXDC_TAB)
return(".error.")
}
# set the deceleration in steps/sec^2
if(_flexdc_wr(dev, sprintf("%sDC=%d", chan, p2))) {
printf("%s ERROR: setting deceleration\n", FLEXDC_TAB)
return(".error.")
}
}
}'
#######################################################################
############################### ###############################
############################### INFOS ###############################
############################### ###############################
#######################################################################
#%UU% motor
#%MDESC%
# Print out all information from the controller concerning the
# specified motor
#
def flexdc_show '{
local mne
local num
local dev
local chan
local ans
local pars[]
local pp
# list of commands and descriptions
pars["CA[36]"]="Min dead zone"
pars["CA[37]"]="Max dead zone"
pars["CA[33]"]="Dead zone bit#1"
pars["MF"]="Motor fault"
pars["MS"]="Motion status"
pars["VL"]="Current velocity"
pars["SP"]="Velocity"
pars["LL"]="Low soft limit"
pars["HL"]="High soft limit"
pars["DL"]="Limit deceleration"
pars["DC"]="Deceleration"
pars["AC"]="Acceleration"
pars["WW"]="Smoothing"
pars["MO"]="Servo loop"
pars["PS"]="Current position"
pars["DP"]="Target position"
# mininum check
if($# != 1) {
print "Usage: $0 motor"
exit
}
# get the motor to snif
mne = "$1"
if((num=motor_num(mne)) == -1) {
print "ERROR: invalid motor: \""mne"\""
exit
}
# check that it is a motor of us
if(motor_par(num, "device_id") != "flexdc") {
print "ERROR: not a FLEXDC motor: \""mne"\""
exit
}
# get the wait to talk to the controller
dev = motor_par(num, "address")
chan = (motor_par(num, "channel")==0)?"X":"Y"
print ""
for(pp in pars) {
ans = _flexdc_query(dev, sprintf("%s%s", chan, pp))
if(ans == "") {
printf("%s ERROR: reading axis status\n", FLEXDC_TAB)
return(".error.")
}
printf("%20s %8s: %s\n", pars[pp], sprintf("[%s]", pp), ans)
}
}'
######################################################################
########################### ##########################
########################### COMMUNICATION ##########################
########################### ##########################
######################################################################
#%IU%(dev, cmd)
#%MDESC%
# Returns string ".error." if no echo from controller
# Returns empty string otherwise
#
def _flexdc_wr(dev, cmd) '{
local ans
# check command terminator
if(substr(cmd, length(cmd), 1) != FLEXDC_EOC) {
cmd=sprintf("%s%s", cmd, FLEXDC_EOC)
}
# flush input queue, just in case
ser_par(dev, "flush")
# send command
ser_put(dev, cmd)
# get command echo
ans = ser_get(dev, FLEXDC_EOC)
if(ans != cmd) {
printf("%s ERROR: missing command echo\n", FLEXDC_TAB)
return(".error.")
}
# normal end
return("")
}'
#%IU%(dev, cmd)
#%MDESC%
# Returns string ".error." if no echo from controller
# Returns empty string if no answer from controller
#
def _flexdc_query(dev, cmd) '{
local ans
# send the command to the controller
if(_flexdc_wr(dev, cmd)) {
return(".error.")
}
# get command answer
ans = ser_get(dev, FLEXDC_PPT)
# remove prompt
if(substr(ans, length(ans), 1) == FLEXDC_PPT) {
ans=substr(ans, 0, length(ans)-1)
}
# normal end
return(ans)
}'
#%MACROS%
#%IMACROS%
#%AUTHOR% MP BLISS (Original 02/2010).
# %BR%$Revision: 1.1 $ / $Date: 2012/08/10 11:57:44 $
#%TOC%
|
