80 lines
2.5 KiB
Tcl
80 lines
2.5 KiB
Tcl
#--------------------------------------------------------------------------
|
|
# test and example script for the anticollider
|
|
#
|
|
# Mark Koennecke, August 2002
|
|
#------------------------------------------------------------------------
|
|
|
|
proc testlimits tg {
|
|
upvar $tg targets
|
|
if { abs( $targets(om) - $targets(stt)) < 30 } {
|
|
error "!!!!! two theta - omega CRASH!!!!!!!"
|
|
}
|
|
if {$targets(chi) > 190.} {
|
|
error "chi upperlimit crashed"
|
|
}
|
|
if { $targets(om) > -90 && $targets(om) <= -81.5 && $targets(chi) < 152} {
|
|
error "!!!!!! chi - omega CRASH aborted !!!!!"
|
|
}
|
|
if { $targets(om) > -81.5 && $targets(om) <= -55 && $targets(chi) < 137} {
|
|
error "!!!!!! chi - omega CRASH aborted !!!!!"
|
|
}
|
|
if { $targets(om) > -55 && $targets(om) <= -52 && $targets(chi) < 132} {
|
|
error "!!!!!! chi - omega CRASH aborted !!!!!"
|
|
}
|
|
if { $targets(om) > -52 && $targets(om) <= -30 && $targets(chi) < 75} {
|
|
error "!!!!!! chi - omega CRASH aborted !!!!!"
|
|
}
|
|
return
|
|
}
|
|
#-------------------------------------------------------------------------
|
|
proc chiFirst? tg {
|
|
upvar $tg targets
|
|
set om [SplitReply [om]]
|
|
set chi [SplitReply [chi]]
|
|
if {$chi < $targets(chi) } {
|
|
return 1
|
|
} else {
|
|
return 0
|
|
}
|
|
}
|
|
#---------------------------------------------------------------------------
|
|
proc collidertest args {
|
|
#----------- read command line targets
|
|
set entries [expr [llength $args] / 2]
|
|
for {set i 0} {$i < $entries} {incr i} {
|
|
set ind [expr $i * 2]
|
|
set targets([lindex $args $ind]) [lindex $args [expr $ind +1]]
|
|
}
|
|
#--------- check if all motors are there. If not get targets from
|
|
# current position
|
|
if { [info exists targets(om)] == 0} {
|
|
set targets(om) [SplitReply [om]]
|
|
}
|
|
if { [info exists targets(stt)] == 0} {
|
|
set targets(stt) [SplitReply [stt]]
|
|
}
|
|
if { [info exists targets(chi)] == 0} {
|
|
set targets(chi) [SplitReply [chi]]
|
|
}
|
|
if { [info exists targets(phi)] == 0} {
|
|
set targets(phi) [SplitReply [phi]]
|
|
}
|
|
#---------- proceed to real collision detection hydraulics
|
|
# first: test complex limits
|
|
set ret [catch {testlimits targets} msg]
|
|
if {$ret != 0} {
|
|
clientput [format "ERROR %s" $msg]
|
|
error $msg
|
|
}
|
|
anticollision add 1 stt $targets(stt)
|
|
|
|
if { [chiFirst? targets] == 1} {
|
|
anticollision add 2 chi $targets(chi)
|
|
anticollision add 3 om $targets(om)
|
|
} else {
|
|
anticollision add 2 om $targets(om)
|
|
anticollision add 3 chi $targets(chi)
|
|
}
|
|
anticollision add 3 phi $targets(phi)
|
|
}
|