diff --git a/site_ansto/instrument/config/robots/sct_pickandplace.tcl b/site_ansto/instrument/config/robots/sct_pickandplace.tcl index 270dd9da..043bc723 100644 --- a/site_ansto/instrument/config/robots/sct_pickandplace.tcl +++ b/site_ansto/instrument/config/robots/sct_pickandplace.tcl @@ -42,6 +42,10 @@ debug_log "setNam new data for $tc_root [sct] result=$data" proc setRot {tc_root nextState cmd} { set data [string toupper [sct target]] + if {$data < 0 || $data > 100} { + sct geterror "Rotation speed must be between 0 and 100, not $data" + return idle + } if {$data != [sct oldval]} { sct oldval $data sct update $data @@ -49,7 +53,7 @@ debug_log "setNam new data for $tc_root [sct] result=$data" debug_log "setRot new data for $tc_root [sct] result=$data" if {$data > 0} { hset $tc_root/status "busy" - sct send "ROTATE $data -1" + sct send "ROTATE $data" } else { sct send "STOPROT" } @@ -79,8 +83,11 @@ debug_log "chkWrite new data for $tc_root [sct] result=$data" proc setPoint {tc_root nextState cmd} { set catch_status [ catch { debug_log "setPoint $tc_root $nextState $cmd sct=[sct]" -debug_log "setPoint: sct=[sct] target=[sct target] writestatus=[sct writestatus]" - sct print "setPoint: sct=[sct] target=[sct target] writestatus=[sct writestatus]" +debug_log "setPoint: sct=[sct] target=[sct target] writestatus=[sct writestatus] driving=[sct driving]" + sct print "setPoint: sct=[sct] target=[sct target] writestatus=[sct writestatus] driving=[sct driving]" + if {[sct driving] == 0} { + sct driving 1 + } set err_msg "" if { [hval $tc_root/Control/Pallet_Nam] != "A" && [hval $tc_root/Control/Pallet_Nam] != "B" } { set err_msg "Invalid Pallet_ Num: [hval $tc_root/Control/Pallet_Nam]" @@ -142,7 +149,6 @@ debug_log "sct send $cmd" hsetprop $tc_root/setpoint driving 0 return -code error $catch_message } - sct print "setPoint: [hget $tc_root/drive_state]" return $nextState } @@ -213,7 +219,7 @@ debug_log "getState returns: $nextState" set nextState idle } else { set my_driving [SplitReply [hgetprop $tc_root/setpoint driving]] - if { $my_driving } { + if { $my_driving || [hval $tc_root/status] != "idle"} { if { [string toupper [sct result]] == "IDLE" } { hset $tc_root/status "idle" hsetprop $tc_root/setpoint driving 0 @@ -379,7 +385,7 @@ debug_log "Registering node $nodeName for write callback" Display Status 1 0 0 text internal {STATUS} {rdValue} {} {} {}\ Display Location 1 0 0 text internal {WHERE} {rdValue} {} {} {}\ Control Pallet_Nam 0 1 0 text user {} {} {} {setNam} {A,B}\ - Control Pallet_Idx 0 1 0 int user {} {} {} {} {}\ + Control Pallet_Idx 0 0 0 int user {} {} {} {} {}\ Control Target_Loc 0 1 0 Text user {} {} {} {setNam} {Beam,Vacuum}\ Control Rotate 0 1 0 int user {} {} {} {setRot} {}\ }