diff --git a/anticollider.c b/anticollider.c index eb5d54cb..7f590847 100644 --- a/anticollider.c +++ b/anticollider.c @@ -99,7 +99,6 @@ static long ColliderSetValue(void *pData, SConnection *pCon, float fTarget){ } iRet = LLDnodePtr2Next(self->motorList); } - printf("%s\n", Tcl_DStringValue(&command)); /* kill old collider sequence diff --git a/collidertest.tcl b/collidertest.tcl new file mode 100644 index 00000000..8eaa0aec --- /dev/null +++ b/collidertest.tcl @@ -0,0 +1,79 @@ +#-------------------------------------------------------------------------- +# 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) +}