#%TITLE% FLEXDC_TCP.MAC
#
#%NAME%
# Macro motors for FlexDc (TCP/IP) controller from Nanomotion
#
#%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 "flexdc_tcp"
# %DT% -
# The "ADDR" must be set to the TCP name of your controller (with port)
# %DT% -
# The "NUM" should be set to 2
# %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 "Chan" field, numbered from 0,
# must be set to the channel on the FLEXDC_TCP controller
# %DT% -
# Optional parameter
# "status_po_cmd" set to non zero if status should wait for power cut (dead zone)
# %XDL%
# %XDL%
#
#%XDL%
#
# In order to configure a macro counter:
# %DL%
# %DT% 1)
# You must have a "SCALERS" defined with:
# %BR%
# %DL%
# %DT% DEVICE
# set to "flexdc_tcp_c"
# %DT% TYPE
# set to "Macro Counter"
# %DT% NUM
# must be set also
# %XDL%
# %DT% 2)
# Per axis you must define a counter with:
# %BR%
# %DL%
# %DT% Device
# set to "MAC_CNT"
# %DT% Mnemonic
# what you want.
# %DT% parameters
# "motor" -> mnemonic of the motor to reflect position.
# %DT% Unit
# do not mind
# %DT% Chan
# do not mind
# %XDL%
#
# %XDL%
#
#
#%END%
#
constant FLEXDC_TCP_TAB "FLEXDC_TCP:"
constant FLEXDC_TCP_EOC "\r"
constant FLEXDC_TCP_PPT ">"
constant FLEXDC_TCP_ACK "Z"
#constant FLEXDC_TCP_WR_SLEEP 0.01
# PC set to 0.002
constant FLEXDC_TCP_WR_SLEEP 0.002
######################################################################
############################ ###########################
############################ MACRO MOTOR ###########################
############################ ###########################
######################################################################
#%IU%
#%MDESC%
# MACRO MOTOR:
# Called by spec after reading the config file
#
def flexdc_tcp_config(num, type, p1, p2, p3) '{
global FLEXDC[]
local dev
local ans
local itf
# p1==?? p2==number of motors supported
if(type=="ctrl") {
# print "Configuring FlexDC TCP controller..."
# Erase any previous configuration
if(p1 == 0) { for(i in FLEXDC) { delete FLEXDC[i] } }
# Gets the IP name to talk to the controller
dev = flexdc_tcp_ADDR
# Try to guess the interface used
if(int(dev) == dev) {
itf = "SERIAL"
} else if(index(dev,"/")) {
itf = "TANGODS"
} else {
itf = "SOCKET"
if(!index(dev,":")) {
dev = dev":4000"
}
FLEXDC[dev]["host"] = dev
}
# Keep a record of the full interface
FLEXDC[dev]["itf"] = itf
printf("%s Looking for controller using interface: %s\n", \
FLEXDC_TCP_TAB, itf)
# Check if socket can be opened.
if(_flexdc_tcp_connect(dev) == ".error.") {
return(".error.")
}
# Checks if the controller is there.
# Disables all the associated motors otherwise.
# Gets controller firmware revision.
ans = _flexdc_tcp_query(dev, "XVR")
if(ans == "") {
printf("%s ERROR: missing answer from controller\n", FLEXDC_TCP_TAB)
return(".error.")
}
printf("%s Found (%s) controller: %s\n", FLEXDC_TCP_TAB, dev, ans)
# normal end
return(0)
}
# p1==unit p2==module p3==channel
if(type=="mot") {
local chan
local cmd
local mne
# Gets the IP name to talk to the controller.
# This access flexdc_tcp_ADDR
dev = motor_par(num, "address")
mne = motor_name(num)
# get the channel (must be 0 or 1)
if((p3<0) || (p3>1)) {
printf("%s ERROR: invalid channel\n", FLEXDC_TCP_TAB)
return(".error.")
}
chan = (p3==0)?"X":"Y"
# print "chan = " chan
# enabling the servo loop, motor is on
printf("%s \"%s\" enabling servo loop on channel %s\n", \
FLEXDC_TCP_TAB, mne, chan)
cmd=sprintf("%sMO=1", chan)
if(_flexdc_tcp_wr(dev, cmd)) {
printf("%s ERROR: Enabling servo loop\n", FLEXDC_TCP_TAB)
return(".error.")
}
# Sets "point to point" motion mode.
# 0 -> point to point
# ( 1 -> jogging ; 2 -> position based gearing )
# ( 5 -> position based ECAM ; 8 -> Step command (no profile) )
cmd=sprintf("%sMM=0", chan)
if(_flexdc_tcp_wr(dev, cmd)) {
printf("%s ERROR: setting \"point to point\" motion mode\n", \
FLEXDC_TCP_TAB)
return(".error.")
}
printf("%s \"%s\" (MM=0) \"point to point\" motion mode\n", \
FLEXDC_TCP_TAB, mne)
# Special motion mode attribute parameter
# 0 -> no special mode
# ( 1 -> repetitive motion )
cmd=sprintf("%sSM=0", chan)
if(_flexdc_tcp_wr(dev, cmd)) {
printf("%s ERROR: setting \"point to point\" motion mode\n", FLEXDC_TCP_TAB)
return(".error.")
}
printf("%s \"%s\" (SM=0) \"point to point\" motion mode\n", \
FLEXDC_TCP_TAB, mne)
# # define no smoothing
# cmd=sprintf("%sWW=0", chan)
# if(_flexdc_tcp_wr(dev, cmd)) {
# printf("%s ERROR: setting no smoothing mode\n", FLEXDC_TCP_TAB)
# return(".error.")
# }
# else{
# printf("%s: (WW=0) no smoothing mode\n", FLEXDC_TCP_TAB)
# }
# Defines smoothing 4
cmd=sprintf("%sWW=4", chan)
if(_flexdc_tcp_wr(dev, cmd)) {
printf("%s ERROR: setting smoothing mode 4 \n", FLEXDC_TCP_TAB)
return(".error.")
}
printf("%s \"%s\" (WW=4) smoothing mode = 4 \n", \
FLEXDC_TCP_TAB, mne)
# Sets acceleration and deceleration
#print "in _config : acc=" motor_par(num, "acceleration")
motor_par(num, "acceleration", motor_par(num, "acceleration"))
# Sets velocity
#print "in _config : velocity=" motor_par(num, "velocity")
motor_par(num, "velocity", motor_par(num, "velocity"))
# By default no closed loop condition checked for end of motion
motor_par(num,"status_cmd","","add")
# Check if closed loop parameters have been set
ans = _flexdc_tcp_query(dev, sprintf("%sTT", chan))
if(ans == "") {
printf("%s ERROR: reading axis TT \n", FLEXDC_TCP_TAB)
return(".error.")
}
if(ans == "0") {
printf("%s WARNING: missing closed loop param TT \n", \
FLEXDC_TCP_TAB)
return
}
ans = _flexdc_tcp_query(dev, sprintf("%sTR", chan))
if(ans == "") {
printf("%s ERROR: reading axis TR \n", FLEXDC_TCP_TAB)
return(".error.")
}
if(ans == "0") {
printf("%s WARNING: missing closed loop param TR \n", \
FLEXDC_TCP_TAB)
return
}
# At this point we should have valid closed loop parameters
motor_par(num,"status_cmd","SR")
# Normal end of axis configuration
return
}
}'
#%IU%
#%MDESC%
# 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_tcp_par(mot_num, key, todo, p1, p2) '{
local dev
local mne
local chan
# nothing to do for the controller
if(mot_num == "..") {
return(0)
}
# get the IP @ to talk to the controller
dev = motor_par(mot_num, "address")
mne = motor_name(mot_num)
# get the channel
chan = (motor_par(mot_num, "channel")==0)?"X":"Y"
#
# set the dead band of controller servo loop (p1=steps)
#
if (key == "dc_dead_band") {
local val
printf("%s \"%s\" set dc_dead_band: %s\n", \
FLEXDC_TCP_TAB, mne, p1)
# activate the dead zone
# get the actual motor position
ans = _flexdc_tcp_query(dev, sprintf("%sCA[33]", chan))
if(ans == "") {
printf("%s ERROR: reading dead zone config\n", FLEXDC_TCP_TAB)
return(".error.")
}
# get the position in steps
if(sscanf(ans, "%d", val) != 1) {
printf("%s ERROR: parsing dead zone config\n", FLEXDC_TCP_TAB)
return(".error.")
}
# set the dead zone min value
cmd=sprintf("%sCA[33]=%d", chan, (val|1))
if(_flexdc_tcp_wr(dev, cmd)) {
printf("%s ERROR: setting dead zone config\n", FLEXDC_TCP_TAB)
return(".error.")
}
# PC: values of min and max dead zone will be equal in all cases?
# set the dead zone min value
cmd=sprintf("%sCA[36]=%d", chan, p1)
if(_flexdc_tcp_wr(dev, cmd)) {
printf("%s ERROR: setting max dead zone\n", FLEXDC_TCP_TAB)
return(".error.")
}
# set the dead zone max value
cmd=sprintf("%sCA[37]=%d", chan, p1)
if(_flexdc_tcp_wr(dev, cmd)) {
printf("%s ERROR: setting max dead zone\n", FLEXDC_TCP_TAB)
return(".error.")
}
}
if (key == "acceleration"){
if(todo == "set"){
# set the acceleration in steps/sec^2
printf("%s \"%s\" set acceleration to: %s\n", \
FLEXDC_TCP_TAB, mne, p1)
if(_flexdc_tcp_wr(dev, sprintf("%sAC=%d", chan, p1))) {
printf("%s ERROR: setting acceleration\n", FLEXDC_TCP_TAB)
return(".error.")
}
# set the deceleration in steps/sec^2
printf("%s \"%s\" set deceleration to: %s\n", \
FLEXDC_TCP_TAB, mne, p1)
if(_flexdc_tcp_wr(dev, sprintf("%sDC=%d", chan, p1))) {
printf("%s ERROR: setting deceleration\n", FLEXDC_TCP_TAB)
return(".error.")
}
}
}
if (key == "slew_rate" || key == "velocity") {
if(todo == "set") {
# set the velocity in steps/sec
printf("%s \"%s\" set velocity to : %s\n", \
FLEXDC_TCP_TAB, mne, p1)
if(_flexdc_tcp_wr(dev, sprintf("%sSP=%d", chan, p1))) {
printf("%s ERROR: setting velecoty\n", FLEXDC_TCP_TAB)
return(".error.")
}
}
}
}'
#%IU%
#%MDESC%
# MACRO MOTOR:
# Called by spec on motor operation.
#
def flexdc_tcp_cmd(mot_num, key, p1, p2) '{
local dev
local mne
local chan
# nothing to do for the controller
if(mot_num == "..") {
return(0)
}
# get the serial to talk to the controller
dev = motor_par(mot_num, "address")
mne = motor_name(mot_num)
# get the channel
chan = (motor_par(mot_num, "channel")==0)?"X":"Y"
#
# return the current motor position in mm or deg
#
if (key == "position") {
local pos
pos = flexdc_tcp_get_pos(mot_num)
return(pos)
}
#
# start a motion (p1==abs pos, p2==rel pos, with pos in mm or deg)
#
if (key == "start_one") {
local pos
pos = flexdc_tcp_set_pos(mot_num, p1)
return(pos)
}
#
# Returns the current motor status.
#
if (key == "get_status") {
local _status
_status = flexdc_tcp_get_status(mot_num)
return(_status)
}
#
# stop a single motor
#
if (key == "abort_one") {
# Stop motion (with deceleration)
print "abort one requested"
# For an abort (without deceleration) use rather the command "AB"
if(_flexdc_tcp_wr(dev, sprintf("%sST", chan))) {
printf("%s ERROR: stopping motion\n", FLEXDC_TCP_TAB)
return(".error.")
}
# if(_flexdc_tcp_wr(dev, sprintf("%sAB", chan))) {
# printf("%s ERROR: stopping motion\n", FLEXDC_TCP_TAB)
# return(".error.")
# }
}
#
# set position (p1=mm) (reset the current position to a given value)
#
if (key == "set_position") {
local pos
# get the absolute position in steps
pos = round_nearest(p1*motor_par(mot_num, "step_size"))
# prepare an absolution motion
if(_flexdc_tcp_wr(dev, sprintf("%sPS=%d", chan, pos))) {
printf("%s ERROR: setting motor position\n", FLEXDC_TCP_TAB)
return(".error.")
}
}
#
# set the velocity (p1=steps/sec)
#
if (key == "slew_rate") {
# set the velocity in steps/sec
if(_flexdc_tcp_wr(dev, sprintf("%sSP=%d", chan, p1))) {
printf("%s ERROR: setting velecoty\n", FLEXDC_TCP_TAB)
return(".error.")
}
}
#
# set the acceleration (p1=mS p2=steps/sec^2
#
if (key == "acceleration") {
# set the acceleration in steps/sec^2
if(_flexdc_tcp_wr(dev, sprintf("%sAC=%d", chan, p2))) {
printf("%s ERROR: setting acceleration\n", FLEXDC_TCP_TAB)
return(".error.")
}
else{
printf("%s set acceleration to %s\n", FLEXDC_TCP_TAB, p2)
}
# set the deceleration in steps/sec^2
if(_flexdc_tcp_wr(dev, sprintf("%sDC=%d", chan, p2))) {
printf("%s ERROR: setting deceleration\n", FLEXDC_TCP_TAB)
return(".error.")
}
else{
printf("%s set deceleration to %s\n", FLEXDC_TCP_TAB, p2)
}
}
if (key == "search") {
# Initiates a home or limit search.
# A prestart_one call will precede this call.
# The parameter p1 indicates the type of search.
# The parameter p2 contains the position in dial units
# that corresponds to the home or limit switch.
print "FLESDC_TCP--flexdc_tcp_cmd()--key=", key, "p1=", p1
if(p1 == "home"){
p " find the home position as appropriate."
}
else if(p1 == "home+"){
p " find home position by moving in positive direction."
}
else if(p1 == "home-"){
p " find home position by moving in negative direction."
}
else if(p1 == "lim+"){
p " find the positive limit."
}
else if(p1 == "lim-"){
p " find the negative limit."
}
else {
print "HMACMOT--hmacmot_cmd()--key=", key," param=", p1, " -> not recognized"
}
}
}'
# This reads the desired position (DP); equal to the absolute position (AP) when no motion is going on
def flexdc_tcp_get_pos(mot_num) '{
local pos_str
local pos
local dev chan
dev = motor_par(mot_num, "address")
chan = (motor_par(mot_num, "channel")==0)?"X":"Y"
# get the actual motor position
ans = _flexdc_tcp_query(dev, sprintf("%sDP", chan))
if(ans == "") {
printf("%s ERROR: reading motor position\n", FLEXDC_TCP_TAB)
return(".error.")
}
# get the position in steps
if(sscanf(ans, "%f", pos) != 1) {
printf("%s ERROR: parsing motor position\n", FLEXDC_TCP_TAB)
return(".error.")
}
# convert the position in mm
pos_str = sprintf("%.15g", pos / motor_par(mot_num, "step_size"))
# normal end
return(pos_str)
}'
# This reads the actual position according to the encoder (PS)
def flexdc_tcp_get_read_pos(mot_num) '{
local pos_str
local pos
local dev chan
dev = motor_par(mot_num, "address")
chan = (motor_par(mot_num, "channel")==0)?"X":"Y"
# get the actual motor position
ans = _flexdc_tcp_query(dev, sprintf("%sPS", chan))
if(ans == "") {
printf("%s ERROR: reading motor position\n", FLEXDC_TCP_TAB)
return(".error.")
}
# get the position in steps
if(sscanf(ans, "%f", pos) != 1) {
printf("%s ERROR: parsing motor position\n", FLEXDC_TCP_TAB)
return(".error.")
}
# convert the position in mm
pos_str = sprintf("%.15g", pos / motor_par(mot_num, "step_size"))
# normal end
return(pos_str)
}'
# This reads the desired speed (SP)
def flexdc_tcp_get_vel(mot_num) '{
local ret
local dev chan
dev = motor_par(mot_num, "address")
chan = (motor_par(mot_num, "channel")==0)?"X":"Y"
# get the set velocity
ans = _flexdc_tcp_query(dev, sprintf("%sSP", chan))
if(ans == "") {
printf("%s ERROR: reading motor speed\n", FLEXDC_TCP_TAB)
return(".error.")
}
# get the value as floating point
if(sscanf(ans, "%f", ret) != 1) {
printf("%s ERROR: parsing motor speed\n", FLEXDC_TCP_TAB)
return(".error.")
}
# normal end
return(ret)
}'
# This reads the actual velocity according to (VL)
def flexdc_tcp_get_read_vel(mot_num) '{
local ret
local dev chan
dev = motor_par(mot_num, "address")
chan = (motor_par(mot_num, "channel")==0)?"X":"Y"
# get the actual velocity
ans = _flexdc_tcp_query(dev, sprintf("%sVL", chan))
if(ans == "") {
printf("%s ERROR: reading actual velocity\n", FLEXDC_TCP_TAB)
return(".error.")
}
# get the value as floating point
if(sscanf(ans, "%f", ret) != 1) {
printf("%s ERROR: parsing actual velocity\n", FLEXDC_TCP_TAB)
return(".error.")
}
# normal end
return(ret)
}'
def flexdc_tcp_get_sr(mot_num) '{
local ret
local dev chan
dev = motor_par(mot_num, "address")
chan = (motor_par(mot_num, "channel")==0)?"X":"Y"
# get the status register
ans = _flexdc_tcp_query(dev, sprintf("%sSR", chan))
if(ans == "") {
printf("%s ERROR: reading status register\n", FLEXDC_TCP_TAB)
return(".error.")
}
# get the value as floating point
if(sscanf(ans, "%f", ret) != 1) {
printf("%s ERROR: parsing status register\n", FLEXDC_TCP_TAB)
return(".error.")
}
# normal end
return(ret)
}'
def flexdc_tcp_get_po(mot_num) '{
local ret
local dev chan
dev = motor_par(mot_num, "address")
chan = (motor_par(mot_num, "channel")==0)?"X":"Y"
# get the power
ans = _flexdc_tcp_query(dev, sprintf("%sPO", chan))
if(ans == "") {
printf("%s ERROR: reading PIV Output\n", FLEXDC_TCP_TAB)
return(".error.")
}
# get the value as floating point
if(sscanf(ans, "%f", ret) != 1) {
printf("%s ERROR: parsing PIV Output\n", FLEXDC_TCP_TAB)
return(".error.")
}
# normal end
return(ret)
}'
def flexdc_tcp_get_pe(mot_num) '{
local ret
local dev chan
dev = motor_par(mot_num, "address")
chan = (motor_par(mot_num, "channel")==0)?"X":"Y"
# get the position error
ans = _flexdc_tcp_query(dev, sprintf("%sPE", chan))
if(ans == "") {
printf("%s ERROR: reading Position Error\n", FLEXDC_TCP_TAB)
return(".error.")
}
# get the value as floating point
if(sscanf(ans, "%f", ret) != 1) {
printf("%s ERROR: parsing Position Error\n", FLEXDC_TCP_TAB)
return(".error.")
}
# normal end
return(ret)
}'
def flexdc_tcp_get_param(mot_num, param) '{
local ret
local dev chan
dev = motor_par(mot_num, "address")
chan = (motor_par(mot_num, "channel")==0)?"X":"Y"
# get the parameter
ans = _flexdc_tcp_query(dev, sprintf("%s%s", chan, param))
if(ans == "") {
printf("%s ERROR: reading %s\n", FLEXDC_TCP_TAB, param)
return(".error.")
}
# get the value as floating point
if(sscanf(ans, "%f", ret) != 1) {
printf("%s ERROR: parsing parameter\n", FLEXDC_TCP_TAB)
return(".error.")
}
# normal end
return(ret)
}'
def flexdc_tcp_set_pos(mot_num, position) '{
local pos
local dev chan
dev = motor_par(mot_num, "address")
chan = (motor_par(mot_num, "channel")==0)?"X":"Y"
## get the absolute position in steps
pos = round_nearest(position * motor_par(mot_num, "step_size"))
## prepare an absolute motion
if(_flexdc_tcp_wr(dev, sprintf("%sAP=%d", chan, pos))) {
printf("%s ERROR: preparing the motion\n", FLEXDC_TCP_TAB)
return(".error.")
}
## launch the motion
if(_flexdc_tcp_wr(dev, sprintf("%sBG", chan))) {
printf("%s ERROR: launching the motion\n", FLEXDC_TCP_TAB)
return(".error.")
}
sleep(0.005)
return(pos)
}'
def flexdc_tcp_set_vel(mot_num, velocity) '{
local dev chan
dev = motor_par(mot_num, "address")
chan = (motor_par(mot_num, "channel")==0)?"X":"Y"
# set the velocity in steps/sec
if(_flexdc_tcp_wr(dev, sprintf("%sSP=%d", chan, velocity))) {
printf("%s ERROR: setting velocity\n", FLEXDC_TCP_TAB)
return(".error.")
}
return(velocity)
}'
def flexdc_tcp_set_param(mot_num, param, val) '{
local dev chan
dev = motor_par(mot_num, "address")
chan = (motor_par(mot_num, "channel")==0)?"X":"Y"
# set param to val
if(_flexdc_tcp_wr(dev, sprintf("%s%s=%d", chan, param, val))) {
printf("%s ERROR: setting %s\n", FLEXDC_TCP_TAB, param)
return(".error.")
}
return(val)
}'
def flexdc_tcp_get_status(mot_num) '{
local _ansMS _ansSR _ansEM _ansPO
local ret
local sta
local dev chan mne
dev = motor_par(mot_num, "address")
chan = (motor_par(mot_num, "channel")==0)?"X":"Y"
mne = motor_name(mot_num)
ret = 0
sta = 0
# get the current axis status
# Motion Status : MS command
# bit 0 : 1 : In motion.
# bit 1 : 2 : In stop.
# bit 2 : 4 : In acceleration.
# bit 3 : 8 : In deceleration.
# bit 4 : 16 : Waiting for input to start motion.
# bit 5 : 32 : In PTP stop (decelerating to target).
# bit 6 : 64 : Waiting for end of WT period.
_ansMS = _flexdc_tcp_query(dev, sprintf("%sMS", chan))
if(_ansMS == "") {
printf("%s ERROR: reading axis status\n", FLEXDC_TCP_TAB)
return(".error.")
}
# get the bitmask from the answer
if(sscanf(_ansMS, "%x", sta) != 1)
{
printf("%s ERROR: unable to get status for: \"%s\"\n", FLEXDC_TCP_TAB, mne)
return(".error.")
}
# get moving status
#if(sta & (1<<0)) { ret |= 0x02; } else {sleep(.2) }
if(sta & (1<<0)) ret |= 0x02;
# SR : Status Register (bit 6 : In-target)
# usable only if TT and TR have been set !
# check if closed loop is still regulating
# PC: Spec can wait for ever here in case PO is set to 0 (dead zone)
# How can this happen? Contradiction between TT/TR and dead zone algorithm?
# Probably when Position Error is below maximum dead zone and above TR
# -> SR will stay at 0
# Maybe we should anyway wait for dead zone (motor unpowered)
# For the moment: track case TR < PE < maximum dead zone and stop with warning
if(ret == 0) {
if(motor_par(mot_num, "status_cmd") !=0 ) {
_ansSR = _flexdc_tcp_query(dev, sprintf("%sSR", chan))
if(_ansSR == "") {
printf("%s ERROR: reading axis status\n", FLEXDC_TCP_TAB)
return(".error.")
}
# get the bitmask from the answer
if(sscanf(_ansSR, "%x", sta) != 1)
{
printf("%s ERROR: unable to get status for: \"%s\"\n", \
FLEXDC_TCP_TAB, mne)
return(".error.")
}
# get moving status
if(!(sta & (1<<5))) ret |= 0x02;
#if (ret != 0) {
# # make sure we are not in pathological case
# _ansPO = _flexdc_tcp_query(dev, sprintf("%sPO", chan))
# if (_ansPO == 0){
# # we are probably in deadzone
# printf ("%s WARNING: Power cut off but not in target.\n", FLEXDC_TCP_TAB)
# ret = 0
# }
#}
}
}
if(ret == 0) {
if(motor_par(mot_num, "status_po_cmd") !=0 ) {
# Make sure power is also cut
_ansPO = _flexdc_tcp_query(dev, sprintf("%sPO", chan))
if(_ansPO == "") {
printf("%s ERROR: reading PO value\n", FLEXDC_TCP_TAB)
return(".error.")
}
if(_ansPO != 0) ret |= 0x02;
}
}
# EM : Last end of motion reason :
# 0 : In motion or after boot up.
# 1 : Last Motion ended normally.
# 2 : Last Motion ended due to Hard FLS.
# 3 : Last Motion ended due to Hard RLS.
# 4 : Last Motion ended due to Soft HL.
# 5 : Last Motion ended due to Soft LL.
# 6 : Last Motion ended due to Motor Fault.
# 7 : Last Motion ended due to User Stop (ST or AB).
# 8 : Last Motion ended due to Motor Off (MO=0).
# 9 : Last Motion ended due to bad ECAM parameters.
# get the current axis status
_ansEM = _flexdc_tcp_query(dev, sprintf("%sEM", chan))
if(_ansEM == "") {
printf("%s ERROR: reading axis status\n", FLEXDC_TCP_TAB)
return(".error.")
}
# get the bitmask from the answer
if(sscanf(_ansEM, "%x", sta) != 1){
printf("%s ERROR: unable to get status for: \"%s\"\n", FLEXDC_TCP_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)
}'
######################################################################
############################### ###############################
############################### INFO ###############################
############################### ###############################
######################################################################
#%UU% motor
#%MDESC%
# Print out all information from the controller concerning the
# specified motor
#
def flexdc_tcp_show(mot_num) '{
local mne
local dev
local chan
local ans
local pars[]
local _param
# list of commands and descriptions
pars["AC"]="Acceleration"
pars["AD"]="Analog Input Dead Band"
pars["AF"]="Analog Input Gain Factor"
pars["AG"]="Analog Input Gain"
pars["AI"]="Analog Input Value"
pars["AP"]="Next Absolute Position Target"
pars["AS"]="Analog Input Offset"
pars["CA[36]"]="Min dead zone"
pars["CA[37]"]="Max dead zone"
pars["CA[33]"]="Dead zone bit#1"
pars["CG"]="Axis Configuration"
pars["DC"]="Deceleration"
pars["DL"]="Limit deceleration"
pars["DO"]="DAC Analog Offset"
pars["DP"]="Desired Position"
pars["EM"]="Last end of motion reason"
pars["ER"]="Maximum Position Error Limit"
pars["HL"]="High soft limit"
pars["IS"]="Integral Saturation Limit"
pars["KD[1]"]="PIV Differential Gain"
pars["KD[2]"]="PIV Differential Gain (Scheduling)"
pars["KI[1]"]="PIV Integral Gain"
pars["KI[2]"]="PIV Integral Gain (Scheduling)"
pars["KP[1]"]="PIV Proportional Gain"
pars["KP[2]"]="PIV Proportional Gain (Scheduling)"
pars["LL"]="Low soft limit"
pars["ME"]="Master Encoder Axis Definition"
pars["MF"]="Motor Fault Reason"
pars["MM"]="Motion mode"
pars["MO"]="Motor On"
pars["MS"]="Motion Status"
pars["NC"]="No Control (Enable open loop)"
pars["PE"]="Position Error"
pars["PO"]="PIV Output"
pars["PS"]="Encoder Position Value"
pars["RP"]="Next Relative Position Target"
pars["SM"]="Special motion mode"
pars["SP"]="Velocity"
pars["SR"]="Status Register"
pars["TC"]="Torque (open loop) Command"
pars["TL"]="Torque Limit"
pars["TR"]="Target Radius"
pars["TT"]="Target Time"
pars["VL"]="Actual Velocity" # Is this true?
pars["WW"]="Smoothing"
# PC : make it a function like others
# mininum check
#if($# != 1) {
# print "Usage: $0 motor"
# exit
# }
# get the motor to snif
#mne = "$1"
#if((mot_num=motor_num(mne)) == -1) {
# print "ERROR: invalid motor: \""mne"\""
# exit
#}
mne = motor_name(mot_num)
# check that it is a motor of us
if(motor_par(mot_num, "device_id") != "flexdc_tcp") {
printf("ERROR: not a FLEXDC_TCP motor: %s", mne)
exit
}
# get the wait to talk to the controller
dev = motor_par(mot_num, "address")
chan = (motor_par(mot_num, "channel")==0)?"X":"Y"
print ""
for(_param in pars) {
ans = _flexdc_tcp_query(dev, sprintf("%s%s", chan, _param))
if(ans == "") {
printf("%s ERROR: reading axis status\n", FLEXDC_TCP_TAB)
return(".error.")
}
printf("%35s %8s: %s\n", pars[_param], sprintf("[%s]", _param), ans)
}
}'
def flexdc_tcp_show_CA(mot_num) '{
local mne
local dev
local chan
local ans
local _ii, ii_max
local _param
ii_min = 1
ii_max = 40
# PC : make it a function like others
# mininum check
#if($# != 1) {
# print "Usage: $0 motor"
# exit
# }
# get the motor to snif
#mne = "$1"
#if((mot_num=motor_num(mne)) == -1) {
# print "ERROR: invalid motor: \""mne"\""
# exit
#}
mne = motor_name(mot_num)
# check that it is a motor of us
if(motor_par(mot_num, "device_id") != "flexdc_tcp") {
printf("ERROR: not a FLEXDC_TCP motor: %s", mne)
exit
}
# get the wait to talk to the controller
dev = motor_par(mot_num, "address")
chan = (motor_par(mot_num, "channel")==0)?"X":"Y"
print ""
for(_ii=ii_min ; _ii<=ii_max; _ii++) {
_param = sprintf("CA[%d]", _ii)
ans = _flexdc_tcp_query(dev, sprintf("%s%s", chan, _param))
if(ans == "") {
printf("%s ERROR: reading axis status\n", FLEXDC_TCP_TAB)
return(".error.")
}
printf("%s: %s\n", _param, ans)
}
}'
######################################################################
############################## #############################
############################## CONTROL #############################
############################## #############################
######################################################################
def flexdc_in_target(mot_num) '{
local _dev _cmd _ans _chan
_dev = motor_par(mot_num, "address")
_chan = (motor_par(mot_num, "channel")==0)? "X" : "Y"
_cmd = sprintf("%sSR", _chan)
_ans = _flexdc_tcp_query(_dev, _cmd)
# Returns state of 6th bit
if(_ans & (1<<5)){
return (1)
}
else{
return (0)
}
}'
#%UU% (<mot_num>)
#%MDESC%
# Runs a homing and wait for completion : in case of Ctrl-C it
# sends an ABORT.
def flexdc_home(mot_num) '{
cdef("cleanup_once", sprintf(";_fdc_abort(%d);", motor_num(mot_num)), "_fdc_home_")
# Asynchronous homing.
flexdc_tcp_home(mot_num)
while( (_status = flexdc_in_target(mot_num)) != 1){
sleep(1)
printf(" homing %s (in-target=%s)\n", motor_mne(mot_num), _status)
}
printf(" homing done %s (in-target=%s)\n", motor_mne(mot_num), _status)
cdef("cleanup_once", "", "_fdc_home_", "delete")
}'
#%UU% (<mot_num>)
#%MDESC%
# Sends a stop command to flex controller axis coresponding to
# motor <mot_num>
def _fdc_abort(mot_num) '{
# print "FDC ABORT"
# flexdc_tcp_cmd(mot_num, "abort_one")
}'
#%UU% (<mot_num>)
#%MDESC%
# Runs the homing procedure.
def flexdc_tcp_home(mot_num) '{
local _dev _cmd _ans _chan
_dev = motor_par(mot_num, "address")
_chan = (motor_par(mot_num, "channel")==0)? "X" : "Y"
_cmd = sprintf("%sQE,#HINX_X", _chan)
printf("_cmd for homing = \"%s\"", _cmd)
_ans = _flexdc_tcp_query(_dev, _cmd)
}'
#######################################################################
########################### ###########################
########################### COMMUNICATION ###########################
########################### ###########################
#######################################################################
#%IU%(dev, cmd)
#%MDESC%
# Sends <cmd> command to <dev> device.
# <cmd> is checked to fix terminators.
# Returns string ".error." if no echo from controller
# Returns empty string otherwise
#
def _flexdc_tcp_wr(dev, cmd) '{
global FLEXDC[]
local itf
local _ans
local _cmd
# printf( "in _flexdc_tcp_wr(%s, %s)\n", dev, cmd)
# Prepare the command
_cmd = cmd
# Checks command terminator (add it if needed).
if(substr(_cmd, length(_cmd), 1) != FLEXDC_TCP_EOC) {
_cmd = sprintf("%s%s", _cmd, FLEXDC_TCP_EOC)
}
# Adds ACK character:
if(substr(_cmd, length(_cmd), 1) != FLEXDC_TCP_ACK) {
_cmd = sprintf("%s%s", _cmd, FLEXDC_TCP_ACK)
}
# Flush input queue, just in case
itf = FLEXDC[dev]["itf"]
if(itf == "SOCKET") {
sock_par(dev, "flush")
} else if(itf == "TANGODS") {
tango_io(dev, "sock_flush")
}
# Sends command
# print "dev= " dev " cmd=" _cmd
if(itf == "SOCKET") {
sock_put(dev, _cmd)
} else if(itf == "TANGODS") {
tango_io(dev, "sock_put", _cmd)
}
# to be adjusted ? PC: needed???
sleep(FLEXDC_TCP_WR_SLEEP)
# to do ?
# # get command echo
# _ans = sock_get(dev, FLEXDC_TCP_EOC)
# if(_ans != _cmd) {
# printf("%s ERROR: missing command echo\n", FLEXDC_TCP_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_tcp_query(dev, cmd) '{
global FLEXDC[]
local itf
local _ans
local _str
local _cmd
# printf ("in _flexdc_tcp_query(%s, %s)\n", dev, cmd)
# Prepare the command
_cmd = cmd
# Checks command terminator (add it if needed).
if(substr(_cmd, length(_cmd), 1) != FLEXDC_TCP_EOC) {
_cmd = sprintf("%s%s", _cmd, FLEXDC_TCP_EOC)
}
# Adds ACK character:
if(substr(_cmd, length(_cmd), 1) != FLEXDC_TCP_ACK) {
_cmd = sprintf("%s%s", _cmd, FLEXDC_TCP_ACK)
}
# Communicate to the controller the request
itf = FLEXDC[dev]["itf"]
if(itf == "SOCKET") {
# Sends the command to the controller.
if(_flexdc_tcp_wr(dev, _cmd)) {
return(".error.")
}
# Gets command answer.
# _ans = sock_get(dev, FLEXDC_TCP_PPT)
_ans = sock_get(dev)
} else if(itf == "TANGODS") {
if((_ans = tango_io(dev,"sock_putget", _cmd)) == -1) {
return(".error.")
}
}
# print _ans
# Removes prompt and ACK.
_str = string_get_last_N_char(_ans, 2)
# print "STR = " _str
if ( _str == sprintf(">%s", FLEXDC_TCP_ACK)){
# print "ack ok"
_ans = string_remove_last_N_chars(_ans, 2)
return (_ans)
}
else{
print "FLEXDC_TCP ERROR : _flexdc_tcp_query : bad ack"
printf("_ans=\"%s\"\n", _ans)
return (".error.")
}
}'
#%IU%(dev)
#%MDESC%
# Try to open a connection to the specified device.
# Returns 0 if success otherwise string ".error.".
#
def _flexdc_tcp_connect(dev) '{
global FLEXDC[]
local itf
local arr[]
local host
itf = FLEXDC[dev]["itf"]
if(itf == "SOCKET") {
if(sock_par(dev, "connect") == 0) {
printf("%s ERROR: missing ADDR field or bad one, no IP @\n", \
FLEXDC_TCP_TAB)
return (".error.")
}
} else if(itf == "TANGODS") {
arr[0] = "connect"
if((host=tango_io(dev, "sock_par", arr)) == -1) {
printf("%s ERROR: missing Tango DS or DS property \"connect\"\n", \
FLEXDC_TCP_TAB)
p TANGO_ERR_STACK
return (".error.")
}
FLEXDC[dev]["host"] = host
} else {
printf("%s ERROR: unsupported interface \"%s\"\n", \
FLEXDC_TCP_TAB, itf)
return (".error.")
}
# Normal end
return(0)
}'
######################################################################
###################### ######################
###################### POSITION MACRO COUNTER ######################
###################### ######################
######################################################################
#%IU%
#%MDESC%
# Called by spec after reading the config file
#
def flexdc_tcp_c_config(count_num, type, p1, p2, p3) '{
#
if(type=="ctrl") {
# ?
}
#
if(type=="cnt") {
local _mot_mne _mot_num _cnt_mne
_cnt_mne = cnt_mne(count_num)
_mot_num = counter_par(count_num, "motor")
_mot_mne = motor_mne(_mot_num)
if((mot_num = motor_num(_mot_mne)) == -1) {
local _msg
_msg = sprintf( "invalid motor name parameter %s for counter %s",\
_mot_mne, _cnt_mne)
printf("%s ERROR: %s\n", FLEXDC_TCP_TAB, _msg)
}
}
}'
#%IU%
#%MDESC%
# Called by spec on counter operation.
#
def flexdc_tcp_c_cmd(count_num, key, p1, p2) '{
if (key == "counts") {
local _pos _mot_num _mot_mne _cnt_mne
_cnt_mne = cnt_mne(count_num)
_mot_num = counter_par(count_num, "motor")
_mot_mne = motor_mne(_mot_num)
_pos = flexdc_tcp_get_read_pos(_mot_num)
return(_pos)
}
}'
#%IU%
#%MDESC%
# Called by spec after reading the config file
#
def flexdc_tcp_vel_config(count_num, type, p1, p2, p3) '{
#
if(type=="ctrl") {
# ?
}
#
if(type=="cnt") {
local _mot_mne _mot_num _cnt_mne
_cnt_mne = cnt_mne(count_num)
_mot_num = counter_par(count_num, "motor")
_mot_mne = motor_mne(_mot_num)
if((mot_num = motor_num(_mot_mne)) == -1) {
local _msg
_msg = sprintf( "invalid motor name parameter %s for counter %s",\
_mot_mne, _cnt_mne)
printf("%s ERROR: %s\n", FLEXDC_TCP_TAB, _msg)
}
}
}'
#%IU%
#%MDESC%
# Called by spec on counter operation.
#
def flexdc_tcp_vel_cmd(count_num, key, p1, p2) '{
if (key == "counts") {
local _pos _mot_num _mot_mne _cnt_mne
_cnt_mne = cnt_mne(count_num)
_mot_num = counter_par(count_num, "motor")
_mot_mne = motor_mne(_mot_num)
_pos = flexdc_tcp_get_read_vel(_mot_num)
return(_pos)
}
}'
#%IU%
#%MDESC%
# Called by spec after reading the config file
#
def flexdc_tcp_sr_config(count_num, type, p1, p2, p3) '{
#
if(type=="ctrl") {
# ?
}
#
if(type=="cnt") {
local _mot_mne _mot_num _cnt_mne
_cnt_mne = cnt_mne(count_num)
_mot_num = counter_par(count_num, "motor")
_mot_mne = motor_mne(_mot_num)
if((mot_num = motor_num(_mot_mne)) == -1) {
local _msg
_msg = sprintf( "invalid motor name parameter %s for counter %s",\
_mot_mne, _cnt_mne)
printf("%s ERROR: %s\n", FLEXDC_TCP_TAB, _msg)
}
}
}'
#%IU%
#%MDESC%
# Called by spec on counter operation.
#
def flexdc_tcp_sr_cmd(count_num, key, p1, p2) '{
if (key == "counts") {
local _pos _mot_num _mot_mne _cnt_mne
_cnt_mne = cnt_mne(count_num)
_mot_num = counter_par(count_num, "motor")
_mot_mne = motor_mne(_mot_num)
_pos = flexdc_tcp_get_sr(_mot_num)
return(_pos)
}
}'
#%IU%
#%MDESC%
# Called by spec after reading the config file
#
def flexdc_tcp_po_config(count_num, type, p1, p2, p3) '{
#
if(type=="ctrl") {
# ?
}
#
if(type=="cnt") {
local _mot_mne _mot_num _cnt_mne
_cnt_mne = cnt_mne(count_num)
_mot_num = counter_par(count_num, "motor")
_mot_mne = motor_mne(_mot_num)
if((mot_num = motor_num(_mot_mne)) == -1) {
local _msg
_msg = sprintf( "invalid motor name parameter %s for counter %s",\
_mot_mne, _cnt_mne)
printf("%s ERROR: %s\n", FLEXDC_TCP_TAB, _msg)
}
}
}'
#%IU%
#%MDESC%
# Called by spec on counter operation.
#
def flexdc_tcp_po_cmd(count_num, key, p1, p2) '{
if (key == "counts") {
local _pos _mot_num _mot_mne _cnt_mne
_cnt_mne = cnt_mne(count_num)
_mot_num = counter_par(count_num, "motor")
_mot_mne = motor_mne(_mot_num)
_pos = flexdc_tcp_get_po(_mot_num)
return(_pos)
}
}'
#%MACROS%
#%IMACROS%
#%AUTHOR% CG+MP BLISS (Original 02/2010).
# %BR%$Revision: 1.3 $ / $Date: 2013/07/01 05:17:22 $
#%TOC%
|
