#%TITLE% NSK.mac
#%NAME%
# Macros for an NSK device (kesako?)
#
#%CATEGORY% Other hardware
#
#%END%
def nsksetup '
{
global NSK_LINES NSK_COUNT NSK_SERLINE NSK_IPOS NSK_DAISY NSK_AXNO
global NSK_OLDSELECT NSK_ACTIVE NSK_OLDSELECT NSK_OLD NSK_CONMOT
global NSK_ALREADYREAD NSK_DEBUG NSK_POSSAVED NSK_SEND
global NSK_FAST
NSK_DEBUG=0
#Set the SPEC serial device for the NSK
NSK_SERLINE=0
#try to get daisy status
NSK_SEND="AS"
nsk_com
nsk_getanswer
if (NSK_COUNT == 0) {
NSK_DAISY = 0
NSK_AXNO = 1
}
else {
NSK_DAISY = 1
NSK_AXNO = NSK_COUNT
}
#report status
printf("I found %d NSK controllers (%s daisy chained)\n",NSK_AXNO,NSK_DAISY?"":"not")
nsk_initvars
for (icc=0;icc<NSK_COUNT;icc++) {
#set NSK controller to multiline mode (doesnt pause after each line!!)
nsk_select icc
NSK_SEND="MM0"
nsk_com
nsk_getanswer
#set NSK controller to position mode
NSK_SEND="PM"
nsk_com
nsk_getanswer
}
}
'
def nsk_select '
if (NSK_DAISY) {
if (NSK_OLDSELECT != $1) {
NSK_SEND=sprintf("AX%d",$1)
nsk_com
if (NSK_DEBUG) {
printf ("*** Select Axis %d \n",$1)
}
nsk_getanswer
NSK_OLDSELECT = $1
}
}'
def nsk_check '
{
local i1 i2
if (NSK_DEBUG) {
print "*** Polling NSK"
}
if (!NSK_FAST) {
NSK_SEND="IO2"
nsk_com
nsk_getanswer
# There is a blank at the beginning of the line
i1=substr(NSK_LINES[0],5,1)
i2=substr(NSK_LINES[0],8,1)
NSK_IPOS =(i1==0)&&(i2==0)
}
else {
NSK_IPOS = 1
}
}
'
def nsk_com '
if (NSK_DEBUG) {
printf( "*** %s\\r sent\n",NSK_SEND)
}
ser_put(NSK_SERLINE,sprintf("%s\r",NSK_SEND))
'
def nsk_getangles '
if (!NSK_ALREADYREAD[$2]) {
nsk_select $2
NSK_SEND="TP2"
nsk_com
nsk_getanswer
sign=motor_par($1,"step_size")/fabs(motor_par($1,"spec_size"))
A[$1]=NSK_LINES[0]/motor_par($1,"spec_size")-sign*180
NSK_ALREADYREAD[$2] = 1
NSK_POSSAVED[$2]=A[$1]
}
else {
A[$1] = NSK_POSSAVED[$2]
}
NSK_OLD[$1] = A[$1]
if (NSK_DEBUG) {
printf( "*** Position motor $1 : %f saved %f read: %d\n",A[$1],NSK_POSSAVED[$2],NSK_ALREADYREAD[$2])
}
'
def nsk_getanswer '
{
local res chain ended
#Check ECHO
nsk_readline
if (chain != NSK_SEND) {
print "I am out of sync - Try to go back"
nsk_sync
exit
}
for (NSK_COUNT=0,ended=0;!ended;NSK_COUNT++) {
nsk_readline
NSK_LINES[NSK_COUNT]=chain
}
NSK_COUNT--
if (NSK_DEBUG) {
print "*** Got back :"
nsk_printanswer
}
}
'
def nsk_readline '
{
local termcount res
nsk_readchar
if (res == ":") {
ended = 1
}
else {
for (chain="",termcount =0; termcount < 1 ;) {
if ((res == "\r") || (res == "\n") ) {
termcount ++ ;
}
else {
chain=sprintf("%s%s",chain,res)
}
nsk_readchar
}
}
}
'
def nsk_readchar '
while ((res=ser_get(NSK_SERLINE,1)) == "" ) {
sleep(.05)
}
'
def nsk_move '
if (NSK_DEBUG) {
printf ("Decide $1 A=%f old=%f %f>%f\n",A[$1],NSK_OLD[$1],fabs(A[$1] - NSK_OLD[$1]),fabs(1/motor_par($1,"step_size")))
}
if ( fabs(A[$1] - NSK_OLD[$1]) > fabs(1/motor_par($1,"step_size")) )
{
local dirs sign
if (NSK_DEBUG) {
print "*** Moving motor $1"
}
NSK_ACTIVE[$1] = 1
nsk_select $2
if (A[$1] > NSK_OLD[$1]) {
dirs="/CW"
}
else {
dirs="/CCW"
}
#
# Added by Olof Svensson 11/12/93
#
if ($1 == th) {
if (NSK_DEBUG) {
print "*** Changing direction of rotation for th"
}
if (dirs == "/CW") {
dirs="/CCW"
}
else {
dirs="/CW"
}
}
#
# Hack to do short moves (< 1deg) with /SD ; By T.Eriksson, H.Graafsma 11/12/93
#
if ( fabs(A[$1] - NSK_OLD[$1]) < 7200.0*fabs(1/motor_par($1,"step_size")) )
{
dirs="/SD"
}
#
sign=motor_par($1,"step_size")/fabs(motor_par($1,"step_size"))
if (NSK_DEBUG) {
print "*** Sending " sprintf("AR%d",(A[$1]+sign*180)*motor_par($1,"step_size"))
}
NSK_SEND=sprintf("AR%d%s",(A[$1]+sign*180)*motor_par($1,"spec_size"),dirs)
nsk_com
NSK_CACTIVE[$2]=1
nsk_getanswer
NSK_ALREADYREAD[$2]=0
if (NSK_FAST) {
NSK_ALREADYREAD[$2]=1
NSK_POSSAVED[$2]=A[$1]
}
}
'
def nsk_poll '
if (NSK_ACTIVE[$1])
{
if (NSK_DEBUG) {
print "*** Waiting for nsk motor $1 No: $2"
}
nsk_select $2
nsk_wait
NSK_ACTIVE[$1] = 0
NSK_CACTIVE[$2] = 0
}
'
def nsk_printanswer '
for (i=0;i<NSK_COUNT;i++) {
print NSK_LINES[i]
}
'
def nsk_wait '
for (NSK_IPOS=0;NSK_IPOS==0;sleep(.05)) {
nsk_check
}
'
def nsk_s ' {
NSK_SEND="$1"
nsk_com
nsk_getanswer
nsk_printanswer
}'
def nsk_assign '
global _pp
local ii
_pp[0]="$1"; _pp[1]="$2"; _pp[2]="$3"; _pp[3]="$4"; _pp[4]="$5";
_pp[5]="$6"; _pp[6]="$7"; _pp[7]="$8"; _pp[8]="$9"; _pp[9]="$10";
_pn = $#
if (!_pn)
{
for (ii=0;ii<NSK_AXNO;ii++)
{
NSK_CONMOT[ii]=getval(sprintf("Motor mnemonic for %d. controller",ii+1),NSK_CONMOT[ii])
}
}
else
{
for (ii=0;ii<NSK_AXNO;ii++)
{
NSK_CONMOT[ii]=_pp[ii]
}
}
for (ii = 0; ii < NSK_AXNO; ii++)
{
local lstr
lstr = sprintf("%s nsk_move nsk_poll nsk_getangles none none none none %d",NSK_CONMOT[ii],ii)
pseudosdef lstr
}
'
def nsk_initvars '
{
global NSK_OLDSELECT NSK_ALREADREAD NSK_CACTIVE NSK_ACTIVE NSK_OLD NSK_FAST
NSK_OLDSELECT = -9999
for (uu=0;uu<NSK_AXNO;uu++) {
NSK_ALREADYREAD[uu]=0
NSK_CACTIVE[uu]=0
}
for (uu=0;uu<MOTORS;uu++) {
NSK_ACTIVE[uu]=0
NSK_OLD[uu]=-99999
}
NSK_FAST=0
}
'
def nsk_emergency '
{
local uu sdone
for (sdone=0,uu=0;uu<NSK_AXNO;uu++) {
if (NSK_CACTIVE[uu]) {
sdone++
nsk_select uu
NSK_SEND="MS"
nsk_com
nsk_getanswer
NSK_CACTIVE[uu]=0
}
}
if (sdone) {
print "Motors stopped (hopefully) - trying to sync again "
nsk_sync
}
}
'
#def cleanup1 '
# nsk_emergency
# '
def nsk_sync '
{
local ereached sillys ended
sillys=sprintf("XXX%d",time())
ser_put(NSK_SERLINE,sprintf("%s\r",sillys))
for (ereached=0;!ereached;) {
nsk_readline
if (NSK_DEBUG) {
print "*** Sync: " chain
}
if (chain == sprintf(" %s?",sillys)) ereached=1 ;
sleep(.05)
}
nsk_readline
if (!ended) {
print "I could not get it to sync - HELP!"
}
}
'
def nsk_par '
{
global uu
nsk_select $1
NSK_SEND=sprintf("FP%f",$2)
nsk_com
nsk_getanswer
for (yy=0;yy<$3;yy++) {
NSK_SEND="TE"
nsk_com
nsk_getanswer
error=NSK_LINES[0]
print "error = " error
data_nput(0,yy,yy,error)
}
ssq= data_anal(0,0,0,0,1,"sumsq")
mval= data_anal(0,0,0,0,1,"sum")/$3
mssq=sqrt(ssq)/sqrt($3)
splot
print "mssq = " mssq ", mval = " mval
data_nput(1,uu,$2,mssq)
data_nput(2,uu++,$2,mval)
}
'
def nsk_scan '
{
global uu tt
uu=0
data_grp(1,4096,2)
data_grp(2,4096,2)
for (tt=$2 ; tt<$3 ; tt=tt+($3-$2)/$4) {
nsk_par $1 tt $5
}
plot_cntl("filter1")
splot 1 0 1
plot_cntl("filter2")
splot 2 0 1
plot_cntl("filter1")
}
'
CP_PRINTER="p4bid11"
def qascan '
{
rdef cleanup \'NSK_FAST = 0\'
NSK_FAST = 1
_sleep = $6
print $1 $2 $3 $4 $5
ascan $1 $2 $3 $4 $5
for (uu=0;uu<NSK_AXNO;uu++) {
if ($1==NSK_CONMOT[uu]) {
NSK_ALREADYREAD[uu]=0
}
}
NSK_FAST=0
undef cleanup
}
'
def qdscan '
{
rdef cleanup \'NSK_FAST = 0\'
NSK_FAST = 1
_sleep = $6
dscan $1 $2 $3 $4 $5
for (uu=0;uu<NSK_AXNO;uu++) {
if ($1==NSK_CONMOT[uu]) {
NSK_ALREADYREAD[uu]=0
}
}
NSK_FAST=0
undef cleanup
}
'
|
