program "%Z% MODULE: %M% RELEASE: %R%.%L% CREATED: %G% %U%"
source "/mdisw/cfl/sim/src/move_legs.src"
descr "move_legs - adjust on-board Alignment Mechanisms (legs)"
# program move_legs.src, created 95.12.14 Igor Zayer 
#
# Part of alignment procedure; adjust on-board Alignment Mechanisms (legs);
# usually called by 'alignit.src'. USE CAUTIOUSLY!!!
# The philosophy is that a real-time (VC2) image MUST be taken before
# adjusting legs.
#
# Calls: checkleglimit, take_pic
# ------
#
# Variable usage:
#----------------
# !GI8: 'error signal nulled' indicator (0 = not nulled, 1 = OK to lock ISS)
# !GS7: 'enable calling the leg routine' string (ensures prerequisites are met)
# !GS9: name of user logfile (actually, the extension only)
#
# !LI0: loop count variable
# !LI1: X-coordinate of image center
# !LI2: Y-coordinate of image center
# !LI3: rounded calculated total motion of AM1
# !LI4: rounded calculated total motion of AM2
# !LI5: commanded motion of AM1
# !LI6: commanded motion of AM2
# !LI7: remaining motion of AM1
# !LI8: remaining motion of AM2
# !LI9: status of AM limitswitches (0: all OK, +-1 for AM1, +-4 for AM2)
# !LR1: starting/projected AM1 pot position
# !LR2: starting/projected AM2 pot position
# !LR3: calculated total motion of AM1
# !LR4: calculated total motion of AM2
# !LR5: auxiliary variable needed in calculation
# !LS0: answer string
# !LS9: status of AM limitswitches - HK tlm query 
#----------------

display "Executing procedure move_legs"
write  !GS9 "Executing procedure move_legs :"
write  !GS9 " %s" !DATE
writen !GS9 " %s" !TIME

## CHECK THAT REQUIREMENTS ARE MET TO CALL THIS PROCEDURE
if !GS7 <> "enable legs" then
  beep
  beep
  display "NOT allowed to run move_legs (no enable string)!!"
  writen !GS9 "procedure not allowed to be run (no enable string)"
  goto done
end if

## CHECK FOR VC2(= DEP Reg.#3 must have a value of 2)
if getint(mksqrg3) <> 2 then
  beep
  beep
  display "Not in VC2 - not allowed to adjust legs!!"
  writen !GS9 "Not in VC2 - not allowed to adjust legs!!"
  goto done
end if

## NEED MORE PICTURES?
menu !LS0 "MORE VERIFICATION PICTURES?" "YES, take some" "NO, just get on!"
if !LS0 = "YES, take some" then 
  perform   take_pic_v01.src
end if

## CHECK IF WE'RE ALREADY CLOSE ENOUGH
if !GI8 = 1 then
  beep
  writen !GS9 "Error signal seems VERY CLOSE..."
  menu !LS0 "Seems CLOSE, give up on legs?" "OK, skip legs" "INSIST on legs!"
  if !LS0 = "OK, skip legs" then 
    goto done
  end if
end if

## ASK FOR PRESENT POINTING
center:
for !LI0 = 0 to 32000
  ask !LI1 "Enter center of image (X-coordinate in 1024 frame):"
  ask !LI2 "Enter center of image (Y-coordinate in 1024 frame):"
  display "Image center (X,Y): %4d" !LI1
  display ", %4d" !LI2 to 25
  menu !LS0 "Is this correct?" "CORRECT" "NO, re-enter" "just QUIT"
  if !LS0 = "just QUIT" then
    goto done
  end if
  exit if !LS0 = "CORRECT"
end for

## CHECK AGAIN IF WE'RE CLOSE ENOUGH
if  !LI1 >= 492 then
  if  !LI1 <= 532 then
    if  !LI2 >= 492 then
      if  !LI2 <= 532 then
        beep
        writen !GS9 "Seems centered enough..."
        menu !LS0 "Seems CLOSE, give up?" "OK, skip legs" "INSIST on legs!"
        if !LS0 = "OK, skip legs" then 
          goto done
        end if
      end if
    end if
  end if
end if

## INSIST ON MOVING LEGS?
menu !LS0 "INSIST on moving the legs?" "YES, I INSIST" "No, give up.."
if !LS0 = "No, give up.." then
  goto done
end if
menu !LS0 "Are You REALLY sure?" "YES, I'm sure!" "No, give up.."
if !LS0 = "No, give up.." then
  goto done
end if

## ISS LOOP MUST BE OPEN!!
if getdig(mkltloop) = "       CLOSE" then
  beep
  beep
  display "You MUST first OPEN the ISS loop!"
  writen !GS9 "You MUST first OPEN the ISS loop!"
  setstr !GS7 = " "
  goto done
end if

## CHECK ENDSWITCHES & POWER
perform   checkleglimit_v01.src
if !GS7 <> "enable legs" then
  goto done
end if

## CALCULATE LEG MOTION --> !LI3, !LI4
set !LI1 = 511 - !LI1		## convert center coord to rel. motion
set !LI2 = 511 - !LI2
set !LR3 = !LI1 * -0.73		## AM1 =- 0.73*DX + 0.998*DY
set !LR5 = !LI2 * 0.998
set !LR3 = !LR3 + !LR5
set !LR4 = !LI1 * -0.761	## AM2 = - 0.761*DX - 0.983*DY
set !LR5 = !LI2 * -0.983
set !LR4 = !LR4 + !LR5
## ROUND OFF PROPERLY AM1
set !LR5 = 0.5
if !LR3 < 0 then
  set !LR5 = -0.5
end if
set !LI3 = !LR3 + !LR5
## ROUND OFF PROPERLY AM2
set !LR5 = 0.5
if !LR4 < 0 then
  set !LR5 = -0.5
end if
set !LI4 = !LR4 + !LR5
display "Projected motion (AM1,AM2):%3d" !LI3
display ",%3d" !LI4 to 31
writen !GS9 "  ..Projected AM1 motion (steps): %4d" !LI3
writen !GS9 "  ..Projected AM2 motion (steps): %4d" !LI4

## CHECK AM POTS & AVAILABLE RANGE
set !LR1 = getreal(mxam1pos)
set !LR2 = getreal(mxam2pos)
writen !GS9 "  Present AM1 pot position: %4.2f" !LR1
writen !GS9 "  Present AM2 pot position: %4.2f" !LR2
set !LR5 = -0.59 * !LI3		## pot1 ~ -0.59 * AM1
set !LR1 = !LR1 + !LR5
set !LR5 = 0.611 * !LI4		## pot2 ~ 0.661 * AM2
set !LR2 = !LR2 + !LR5
if !LR1 > 217.0 then
  beep
  display "Sorry, You'd be running into the AM1 decrement limit!"
  writen !GS9 "  Would reach the AM1 increment limit - quitting!"
  goto done
end if
if !LR1 < -246.0 then
  beep
  display "Sorry, You'd be running into the AM1 increment limit!"
  writen !GS9 "  Would reach the AM1 decrement limit - quitting!"
  goto done
end if
if !LR2 > 117.0 then
  beep
  display "Sorry, You'd be running into the AM2 increment limit!"
  writen !GS9 "  Would reach the AM2 increment limit - quitting!"
  goto done
end if
if !LR2 < -306.0 then
  beep
  display "Sorry, You'd be running into the AM2 decrement limit!"
  writen !GS9 "  Would reach the AM2 decrement limit - quitting!"
  goto done
end if

## FINALLY MOVE THE LEGS!!
set !LI7 = !LI3
set !LI8 = !LI4
## DO IT IN LITTLE STEPS...
continue:
if !LI7 > 0 then 
  set !LI5 = !LI7 
else 
  set !LI5 = -1 * !LI7
end if
if !LI5 > 25 then 
  set !LI5 = 25 
end if 
if !LI8 > 0 then 
  set !LI6 = !LI8 
else 
  set !LI6 = -1 * !LI8
end if
if !LI6 > 25 then 
  set !LI6 = 25 
end if 
menu !LS0 "Do it?" "YES, move legs.." "Something's wrong, QUIT!"
if !LS0 = "Something's wrong, QUIT!" then 
   goto done
end if
iif !LI7 > 0 then
  display "Sending: am1inc %3d" !LI5 to 38
  writen !GS9 "  --> Sending: am1inc %4d" !LI5
  tcblock mbam1inc !LI5
  set !LI7 = !LI7 - !LI5
end if
if !LI7 < 0 then
  display "Sending: am1dec %3d" !LI5 to 38
  writen !GS9 "  --> Sending: am1dec %4d" !LI5
  tcblock mbam1dec !LI5
  set !LI7 = !LI7 + !LI5
end if
if !LI8 > 0 then
  display "Sending: am2inc %3d" !LI6 to 58
  writen !GS9 "  --> Sending: am2inc %4d" !LI6
  tcblock mbam2inc !LI6
  set !LI8 = !LI8 - !LI6
end if
if !LI8 < 0 then
  display "Sending: am2dec %3d" !LI6 to 58
  writen !GS9 "  --> Sending: am2dec %4d" !LI6
  tcblock mbam2dec !LI6
  set !LI8 = !LI8 + !LI6
end if
###sync hk###
writen !GS9 "  Resulting AM1 pot position: %4.2f" getreal(mxam1pos)
writen !GS9 "  Resulting AM2 pot position: %4.2f" getreal(mxam2pos)
## SEE HOW WE'RE DOING...
setstr !GS7 = " "	## disable continuation if picture fails..
perf   take_pic_v01.src
menu !LS0 "Are we getting closer?" "YES, continue" "Something's wrong, QUIT!"
if !LS0 = "Something's wrong, QUIT!" then 
  display " legs don't move in the right direction!!"
  writen !GS9 " legs don't move in the right direction - quitting!!"
  setstr !GS7 = "  "  ## disable calling until meeting prerequisites again
  goto done
end if
if !LI7 <> 0 then
  goto continue
end if
if !LI8 <> 0 then
  goto continue
end if

## SEE WHERE WE'VE ARRIVED!
display "Leg adjustment completed..."
perf   take_pic_v01.src
menu !LS0 "Is the picture centered?" "YES / QUIT" "NO, try again"
if !LS0 = "NO, try again" then 
  goto center
end if

done:
setstr !GS7 = " "	## disable calling this procedure right away
writen !GS9 "Finished procedure move_legs : %s" !TIME
writen !GS9 " "
display "Finished Procedure move_legs                                   "
return
