- Working version of a anticollision system

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

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)
+}