makeasyncqueue acq ORHVPS 137.157.202.85 4001
evfactory new dhv1 orhvps acq
dhv1 lowerlimit 0
dhv1 upperlimit 2400
dhv1 tolerance 19
dhv1 max 2400
dhv1 rate 10

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