#%TITLE% simulmot
#$Revision: 1.7 $
#%NAME%
#%DESCRIPTION%
# Versions >1.3 fix a bug in the macro simulon when started with all arguments (simulon motor home lhwlimit hhwlimit)
# Versions >1.3 include 2 new commands %B%simul_enable_hwl%B% and %B%simul_disable_hwl%B% to enabble or disable
# the harware limit switches. These values are set by the command %B% simulon motor home_pos low_hw_limit high_hw_limit%B%
# If one forget to set theses values in the setup file when Spec is started from fresh it will be impossible to move a simulation motor raising the error:
# Both hard limits active on motor 0, unit 0, "motor_name"
# If you don't want to set these values using simulon you can put the command %B%simul_disable_hwl motor_name%B% in your setup file.
# Gilbert 04/11/2009
#%SETUP%
#%IU%()
#%MDESC%
def simulglobals() '{
global SIMULMNE
global SIMULSTARTTIME[]
global SIMULTARGET[]
global SIMULMOVING[]
global SIMULSTARTPOS[]
global SIMULRGS[]
global SIMULRGT[]
global SIMULRGV[]
global SIMULRGA[]
global SIMULDIR[]
global SIMULHOMEPOS[]
global SIMULPLIMIT[]
global SIMULNLIMIT[]
global SIMULPOS[]
global SIMULNOHWL[]
}'
#%IU%()
#%MDESC%
def simulcglobals() '{
global SIMULCMNE
global SIMULCCOUNTING[]
global SIMULCTARGET[]
global SIMULCCOUNTS[]
global SIMULCMODE[]
global SIMULCSTARTTIME[]
global SIMULCFREQ[]
}'
#%UU%()
#%MDESC%
def simulon '{
local m, mnum, hpos, llim, ulim
simulglobals()
if ( $# != 4) {
m = getval("Mnemonic for simulated motor controller",SIMULMNE)
if ((mnum = motor_num(m)) == -1) {print "Unknown mnemonic ", m ; exit}
hpos = getval("Home position",motor_par(mnum, "home_pos"))
llim = getval("Lower hard limit",motor_par(mnum, "low_hard_limit"))
ulim = getval("Upper hard limit",motor_par(mnum, "high_hard_limit"))
motor_par(mnum, "home_pos", hpos)
motor_par(mnum, "low_hard_limit", llim)
motor_par(mnum, "high_hard_limit", ulim)
SIMULPOS[m] = usertosteps(mnum, A[mnum])
SIMULMOVING[m] = 0
SIMULMNE = m
SIMULNOHWL[motor_num(m)]=0
exit
}
m = "$1" ; hpos = $2 ; llim = $3; ulim = $4
mnum=motor_num(m)
if (mnum == -1) {
printf ("Simulon from the setup file: Unknown motor mnemonic %s ",m)
exit
}
motor_par(mnum, "home_pos", hpos)
motor_par(mnum, "low_hard_limit", llim)
motor_par(mnum, "high_hard_limit", ulim)
SIMULPOS[m] = usertosteps(mnum, A[mnum])
SIMULMOVING[m] = 0
SIMULMNE = m
SIMULNOHWL[motor_num(m)]=0
}'
#%IU%()
#%MDESC%
def simulcon '{
local c, cnum
simulcglobals()
c = getval("Mnemonic for simulated counter", SIMULCMNE)
cnum = cnt_num(c)
if (mnum != -1) {
SIMULCCOUNTING[c] = 0
SIMULCFREQ[c] = 10000
SIMULCMNE = c
} else
eprint "Unknown mnemonic ", c
}'
#%IU%(mnum, vuser)
#%MDESC%
def usertosteps(mnum, vuser) '{
return dial(mnum, vuser) * motor_par(mnum, "step_size")
}'
#%IU%(mnum, steps)
#%MDESC%
def stepstouser(mnum, steps) '{
return user(mnum, steps / motor_par(mnum, "step_size"))
}'
#%IU%(mnum, vdial)
#%MDESC%
def dialtosteps(mnum, vdial) '{
return vdial * motor_par(mnum, "step_size")
}'
#%IU%(mnum, steps)
#%MDESC%
def stepstodial(mnum, steps) '{
return steps / motor_par(mnum, "step_size")
}'
# Use one of the following command codes
# base_rate
# slew_rate
# bvel_rate
# accel
# home_base_rate
# home_slew_rate
# home_accel
# backlash
# magnitude
# search
# get_status must return bitfields 2/4/8
# position
# set_position
# diff_position
# prestart_all
# prestart_one
# start_one
# start_all
# abort_all
# abort_one
# flush_all
# flush_one
# step_size
# sign
# offset
# high_lim
# low_lim
# move_done
# high_lim_hit
# low_lim_hit
# encoder_step_size
# sync_calcheck
# unusable
# dial
# preread_all
# preread_one
# dc_dead_band
# dc_settle_time
# dc_proportional_gain
# dc_derivative_gain
# dc_integral_gain
# dc_integration_imit
# dc_following_error
# dc_sampling_interval
# step_mode
# slop
# read_mode
# deceleration
# torque
# misc_1
# misc_2
# misc_3
# misc_4
# misc_5
# misc_6
# For search the following values for p1 are given
# home_search
# home_plus
# home_minus
# plus_search
# minus_search
# home_p_suggest
# home_m_suggest
# mot_plus_limit
# mot_nega_limit
#%IU%(mne, what, p1, p2)
#%MDESC%
def simul_cmd(mne, what, p1, p2) '{
simulglobals()
local mnum, pos, status
# ".." is for all, commands like preread_all or start_all ...
# not supported here just return
if (mne == "..") {return}
mnum = motor_num(mne)
if (what == "get_status") {
pos = simul_getpos(mne)
if (SIMULMOVING[mne])
status = 2
else
status = 0
if (fabs(pos - SIMULPLIMIT[mne]) < 0.5)
status |= 4
if (fabs(pos - SIMULNLIMIT[mne]) < 0.5)
status |= 8
return status
} else if (what == "position") {
return stepstodial(mnum,simul_getpos(mne))
} else if (what == "start_one") {
simul_startmove(mne, p1)
} else if (what == "abort_one") {
SIMULPOS[mne] = simul_getpos(mne)
SIMULMOVING[mne] = 0
return 1
} else if (what == "set_position") {
relset = dialtosteps(mnum, p1) - SIMULPOS[mne]
SIMULNLIMIT[mne] += relset # Adapt the limits to the new pos
SIMULPLIMIT[mne] += relset
SIMULHOMEPOS[mne] += relset
SIMULPOS[mne] = dialtosteps(mnum, p1)
} else if (what == "search") {
if (substr(p1,0,4) == "home") {
local br, sr, acc_t_in_s
SIMULSTARTPOS[mne] = SIMULPOS[mne]
SIMULSTARTTIME[mne] = time()
SIMULTARGET[mne]=SIMULHOMEPOS[mne]
br = motor_par(mnum, "home_base_rate")
if (br == 0) {
br = motor_par(mnum, "base_rate")
sr = br
} else {
sr = motor_par(mnum, "home_slew_rate")
acc_t_in_s = motor_par(mnum, \
"home_acceleration") / 1000
if (sr == 0)
sr == br
}
simul_calcregions(mne, SIMULHOMEPOS[mne] \
- SIMULPOS[mne], br, sr, acc_t_in_s)
SIMULMOVING[mne] = 1
} else if (p1 == "lim+") {
simul_startmove(mne, 1E10)
} else if (p1 == "lim-") {
simul_startmove(mne, -1E10)
}
}
}'
#%IU%(mne, key, action, p1)
#%MDESC%
def simul_par(mne, key, action, p1) '{
simulglobals()
local mnum
mnum = motor_num(mne)
if (key == "low_hard_limit") {
if (action == "set")
SIMULNLIMIT[mne] = dialtosteps(mnum,p1)
return stepstodial(mnum,SIMULNLIMIT [mne])
} else if (key == "high_hard_limit") {
if (action == "set")
SIMULPLIMIT[mne] = dialtosteps(mnum,p1)
return stepstodial(mnum,SIMULPLIMIT [mne])
} else if (key == "home_pos") {
if (action == "set")
SIMULHOMEPOS[mne] = dialtosteps(mnum,p1)
return stepstodial(mnum,SIMULHOMEPOS[mne])
} else if (key == "?") {
print "low_hard_limit", "high_hard_limit", "home_pos"
} else if (key == "dump") {
print "low_hard_limit :", stepstodial(mnum,SIMULNLIMIT [mne])
print "high_hard_limit :", stepstodial(mnum,SIMULPLIMIT [mne])
print "home_pos :", stepstodial(mnum,SIMULHOMEPOS[mne])
}
}'
#%IU%(mne, code, p1, p2)
#%MDESC%
# code is mot ctrl detach
def simul_config(mne, code, p1, p2) '{
simulglobals()
mnum = motor_num(mne)
if (code == "mot") {
SIMULMOVING[m] = 0
if (SIMULPLIMIT[mne] == SIMULNLIMIT[mne]) {
SIMULPLIMIT[mne] = dialtosteps(mnum,\
motor_par(mnum, "high_limit")*.99)
SIMULNLIMIT[mne] = dialtosteps(mnum,\
motor_par(mnum, "low_limit")*.99)
SIMULPOS[mne] = 0
}
}
}'
#%IU%(mne, totalmove, baserate, slewrate, acctime)
#%MDESC%
# Calculate the different regions (acc/constant/decel) in pos and time
def simul_calcregions(mne, totalmove, baserate, slewrate, acctime) '{
simulglobals()
abstotal = fabs(totalmove)
if (acctime < 0.001) {
SIMULRGS[mne][1] = SIMULRGS[mne][0] = 0
SIMULRGT[mne][1] = SIMULRGT[mne][0] = 0
SIMULRGV[mne][1] = SIMULRGV[mne][0] = baserate
SIMULRGA[mne][1] = SIMULRGA[mne][0] = 0
SIMULRGS[mne][3] = SIMULRGS[mne][2] = abstotal
SIMULRGT[mne][3] = SIMULRGT[mne][2] = abstotal / baserate
SIMULRGV[mne][3] = SIMULRGV[mne][2] = baserate
SIMULRGA[mne][3] = SIMULRGA[mne][2] = 0
} else {
acc = (slewrate - baserate) / acctime
sacc = simul_calc_real_acc_s(totalmove, baserate, slewrate, acctime)
SIMULRGS[mne][0] = 0
SIMULRGT[mne][0] = 0
SIMULRGV[mne][0] = baserate
SIMULRGA[mne][0] = acc
SIMULRGS[mne][1] = sacc
SIMULRGT[mne][1] = sqrt(2 * sacc/acc + pow(baserate/acc,2)) - baserate/acc
SIMULRGV[mne][1] = baserate + SIMULRGT[mne][1] * acc
SIMULRGA[mne][1] = 0
SIMULRGS[mne][2] = abstotal - sacc
SIMULRGT[mne][2] = SIMULRGT[mne][1] + (abstotal-2*sacc) / SIMULRGV[mne][1]
SIMULRGV[mne][2] = SIMULRGV[mne][1]
SIMULRGA[mne][2] = acc
SIMULRGS[mne][3] = abstotal
SIMULRGT[mne][3] = SIMULRGT[mne][2] + SIMULRGT[mne][1]
SIMULRGV[mne][3] = baserate
SIMULRGA[mne][3] = 0
}
if (fabs(totalmove)> 0.001)
SIMULDIR[mne] = totalmove / fabs(totalmove)
else
SIMULDIR[mne] = 1
}'
#%IU%(s, baserate, slewrate, acctime)
#%MDESC%
# Calculate the real length of the acceleration phase
def simul_calc_real_acc_s(s, baserate, slewrate, acctime) '{
sacc = (slewrate + baserate) * acctime / 2
if (sacc > fabs(s)/2)
sacc = fabs(s)/2
return sacc
}'
#%UU% motor_name
#%MDESC%
# Disable the hardware limits for a motor
def simul_disable_hwl '{
simulglobals()
local mne
if($# !=1) { p " Usage: simul_disable_hwl motor_name"}
mne = "$1"
if(motor_num(mne) == -1) {
printf("OOoops %s is not a motor defined in the config \n",mne)
exit
} else {
SIMULNOHWL[motor_num(mne)]=1
}
}'
#%UU% motor_name
#%MDESC%
# Enable the hardware limits for a motor
def simul_enable_hwl '{
local mne
if($# !=1) { p " Usage: simul_enable_hwl motor_name"}
mne = "$1"
if(motor_num(mne) == -1) {
printf("OOoops %s is not a motor defined in the config \n",mne)
exit
} else {
SIMULNOHWL[motor_num(mne)]=0
}
}'
#%IU%(mne)
#%MDESC%
# Return the actual position (from global variables)
def simul_getpos(mne) '{
simulglobals()
if (SIMULMOVING[mne] == 1) {
if (time() - SIMULSTARTTIME[mne] > SIMULRGT[mne][3]) {
SIMULMOVING[mne] = 0
SIMULPOS[mne] = SIMULTARGET[mne]
} else {
SIMULPOS[mne] = SIMULSTARTPOS[mne] + SIMULDIR[mne] * \
simul_calcpos(mne, time() - SIMULSTARTTIME[mne])
}
if ((SIMULNOHWL[mne] != 1) && (SIMULPOS[mne] > SIMULPLIMIT[mne])) {
SIMULPOS[mne] = SIMULPLIMIT[mne]
SIMULMOVING[mne] = 0
}
if ((SIMULNOHWL[mne] != 1) && (SIMULPOS[mne] < SIMULNLIMIT[mne])) {
SIMULPOS[mne] = SIMULNLIMIT[mne]
SIMULMOVING[mne] = 0
}
}
return SIMULPOS[mne]
}'
#%IU%(mne, target)
#%MDESC%
def simul_startmove(mne, target) '{
simulglobals()
mnum = motor_num(mne)
SIMULSTARTTIME[mne] = time()
SIMULSTARTPOS[mne] = SIMULPOS[mne]
SIMULTARGET[mne] = dialtosteps(mnum, target)
simul_calcregions(mne, dialtosteps(mnum, target) - SIMULPOS[mne], \
motor_par(mnum, "base_rate"), \
motor_par(mnum, "velocity"), \
motor_par(mnum, "acceleration") / 1000)
SIMULMOVING[mne] = 1
}'
#%IU%(mne,t)
#%MDESC%
# Return the position when time is given
def simul_calcpos(mne, t) '{
simulglobals()
for (i=0; i<3; i++) {
if (t < SIMULRGT[mne][i+1]) {
dt = t - SIMULRGT[mne][i]
pos = SIMULRGS[mne][i] + SIMULRGV[mne][i]*dt \
+ SIMULRGA[mne][i] / 2 * dt * dt
return pos
}
}
return SIMULRGS[mne][3]
}'
#%IU%(mne, what, p1, p2)
#%MDESC%
#ct_prestart_all
#ct_prestart_one
#ct_start_one time mode (1 = monitor, 2 = time, 3 = continious)
#ct_start_all
#ct_get_status
#ct_counts should return the current counter values
# Simulated COUNTER
def simulc_cmd(mne, what, p1, p2) '{
simulglobals()
mnum = motor_num(mne)
if (what == "start_one") {
SIMULCCOUNTING[mne] = 1
SIMULCTARGET[mne] = p1
SIMULCCOUNTS[mne] = 0
SIMULCMODE[mne] = p2
SIMULCSTARTTIME[mne] = time()
if (SIMULCFREQ[mne] == 0)
SIMULCFREQ[mne] = 10000
} else if (what == "get_status") {
if (SIMULCMODE[mne] == 2) {
if (time() - SIMULCSTARTTIME[mne] > SIMULCTARGET[mne]) {
SIMULCCOUNTING[mne] = 0
return 0
} else
return 1
} else if (SIMULCMODE[mne] == 1) {
simulc_updatecounts(mne)
if (SIMULCCOUNTS[mne] > SIMULCTARGET[mne]) {
SIMULCCOUNTS[mne] = SIMULCTARGET[mne]
SIMULCCOUNTING[mne] = 0
}
return SIMULCCOUNTING[mne]
} else {
simulc_updatecounts(mne)
return 0
}
} else if (what == "counts") {
c = cnt_num(mne)
if (SIMULCMODE[mne] == 2) {
if (SIMULCCOUNTING[mne]) {
return (time() - SIMULCSTARTTIME[mne]) * counter_par(c, "scale")
} else
return SIMULCTARGET[mne] * counter_par(c, "scale")
} else {
if (SIMULCCOUNTING[mne]) {
simulc_updatecounts(mne)
}
}
return SIMULCCOUNTS[mne]
}
}'
#%IU%(mne, key, action, p1)
#%MDESC%
def simulc_par(mne, key, action, p1) '{
simulcglobals()
if (key == "frequency") {
if (action == "set")
SIMULCFREQ[mne] = p1
return SIMULCFREQ[mne]
} else if (key == "?") {
print "frequency "
}
}'
#%IU%(mne, code, p1, p2)
#%MDESC%
def simulc_config(mne, code, p1, p2) '{
simulcglobals()
if (SIMULCFREQ[mne] == 0 && code == "cnt") {
SIMULCCOUNTING[mne] = 0
SIMULCFREQ[mne] = 10000
}
}'
#%IU%(mne)
#%MDESC%
def simulc_updatecounts(mne) '{
simulcglobals()
delta_t = time() - SIMULCSTARTTIME[mne]
delta_c = SIMULCFREQ[mne] * delta_t * (1 + rand(-1000)/10000)
SIMULCCOUNTS[mne] += delta_c
SIMULCSTARTTIME[mne] = time()
}'
# real: sl = neg blade
# sr = pos blade
# pseudo: sg = gap
# so = offset
def ptest_config(mne, type, p1, p2) '{
if (type == "mot")
return "sl sr"
}'
def ptest_calc(mne, mode, mne2) '{
# printf("ptest_calc(%s, %d%s%s)\n", mne,mode,mne2? ", ":"", mne2)
if (mode == 0) {
if (mne == "sg")
A[sg] = A[sl] + A[sr]
if (mne == "so")
A[so] = (A[sr] - A[sl])/2
} else {
if (mne2 == "sl")
A[sl] = -A[so] + A[sg]/2
if (mne2 == "sr")
A[sr] = A[so] + A[sg]/2
}
}'
#%MACROS%
#%IMACROS%
#%AUTHOR%
#%TOC%
|
