Add mechanism to pop up beam stops small(1) to (large(5)

r2281 | dcl | 2008-01-15 12:39:04 +1100 (Tue, 15 Jan 2008) | 2 lines
This commit is contained in:
Douglas Clowes
2008-01-15 12:39:04 +11:00
parent cad1e0a4d5
commit b8c0f9ac5b

View File

@@ -0,0 +1,50 @@
proc TrimReply { str } {
set reply [string trim $str " :"]
return $reply
}
proc pop { which } {
set down { -1 1 -1 1 1 }
set speed 50000
set motions { 0 0 0 0 0 }
set indices { 0 1 2 3 4 }
set motors { BS1 BS2 BS3 BS4 BS5 }
if {($which < 1)||($which > [llength $indices])} {
clientput "Pop must be between 1 and [llength $indices]"
return
}
incr which -1
foreach i $indices {
lset motions $i [expr $speed * [lindex $down $i]]
}
lset motions $which [expr -1 * [lindex $motions $which]]
foreach i $indices { [lindex $motors $i] send SH` }
foreach i $indices { [lindex $motors $i] send JG`=[lindex $motions $i] }
foreach i $indices { [lindex $motors $i] send BG` }
set x 0
set started [clock seconds]
while { $x == 0 } {
set x 1
set current [clock seconds]
foreach i $indices {
if { [TrimReply [[lindex $motors $i] send MG _BG`]] > 0 } {
set x 0
if { ([expr $current - 60] > $started) } {
clientput [lindex $motors $i] send ST`
[lindex $motors $i] send ST`
}
}
}
}
foreach i $indices {
set forward [TrimReply [[lindex $motors $i] send MG _LF`]]
set reverse [TrimReply [[lindex $motors $i] send MG _LR`]]
if { [expr $forward * $reverse] > 0 } {
clientput "[lindex $motors $i] is between limits"
}
}
clientput "POP [expr $which + 1] took [expr $current -$started] seconds"
}
publish pop user