###CHECKPOINT ARGS:  -nostartlog -nolog-flow -nolog-blocks -queue=0
program "%Z% MODULE: %M% RELEASE: %R%.%L% CREATED: %G% %U%"
source "/mdisw/cfl/flight/src/move_mirror_v02.src"
descr "move_mirror - adjust on-board Alignment Mechanisms (legs)"
# program move_mirror.src, created 96.04.15 Igor Zayer 
#
#	96.04.16  (IZ)	- removed requirement for VC2 & option to take picture
#
# Part of alignment procedure; adjust image very finely using PZT offsets;
# usually called by 'alignit.src'. 
# The philosophy is that real-time (VC2) telemetry MUST be available ...
#
# Calls: take_pic1x1
# ------
#
# Variable usage:
#----------------
# !GS9: name of user logfile (actually, the extension only)
#
# !LI0: loop count variable
# !LI1: commanded PZT-A offset / auxiliary ask variable
# !LI2: commanded PZT-B offset / auxiliary ask variable
# !LI3: commanded PZT-C offset
# !LI9: status of planned PZT offsets (0: in range; 1: out of range)
# !LR1: desired x-offset (FD pix) / auxiliary variable
# !LR2: desired x-offset (FD pix) / auxiliary variable
# !LR5: auxiliary variable needed in calculation
# !LS0: answer string
# !LS9: status of AM limitswitches - HK tlm query 
#----------------

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

## ISS LOOP MUST BE OPEN!!
if getdig(mkltloop) = "       CLOSE" then
  beep
  beep
  display "ISS loop MUST be OPEN - opening loop.."
  writen !GS9 " opening ISS loop first.."
  tcb mbltopnl
end if

## SHOULD START WITH "HOME" POSITION
display "Making sure we start from HOME position..."
set !LI9 = 0
if getint(mkltaos) <> 63 then
  set !LI9 = 1
end if
if getint(mkltbos) <> 63 then
  set !LI9 = 1
end if
if getint(mkltcos) <> 63 then
  set !LI9 = 1
end if
if !LI9 = 1 then
  queue on
  tcblock mbltaofs 0
  tcblock mbltbofs 0
  tcblock mbltcofs 0
  tcblock mbltaofs 63
  tcblock mbltbofs 63
  queue off
  tcblock mbltcofs 63
end if

## ASK FOR PRESENT POINTING
center:
for !LI0 = 0 to 32000
  ask !LI1 "Enter desired X-offset in 1/100 of FD pix:"
  ask !LI2 "Enter desired Y-offset in 1/100 of FD pix:"
  set !LR1 = !LI1 / 100.
  set !LR2 = !LI2 / 100.
  display "Displace (dX,dY): %5.2f" !LR1
  display ", %5.2f FD pixels" !LR2 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

## CALCULATE NEW OFFSETS (using Luiz' equations)
set !LR1 = !LR1 * 7.4238
set !LR2 = !LR2 * 4.6904
set !LI1 = 63.5 - !LR1
set !LR1 = !LR1 / 2.0
set !LR5 = !LR1 + !LR2
set !LI2 = 63.5 + !LR5
set !LR5 = !LR1 - !LR2
set !LI3 = 63.5 + !LR5

## CHECK ALLOWABLE PZT RANGE
set !LI9 = 0
if !LI1 < 18 then
  set !LI9 = 1
end if
if !LI1 > 109 then
  set !LI9 = 1
end if
if !LI2 < 18 then
  set !LI9 = 1
end if
if !LI2 > 109 then
  set !LI9 = 1
end if
if !LI3 < 18 then
  set !LI9 = 1
end if
if !LI3 > 109 then
  set !LI9 = 1
end if
if !LI9 = 1 then
  beep
  beep
  display " Sorry, exceeds PZT range - use legs instead!"
  menu !LS0 "Enter new offsets?" "YES, try again" "NO, just QUIT!"
  if !LS0 = "YES, try again" then 
    goto center
  end if
  if !LS0 = "NO, just QUIT!" then 
    goto done
  end if
end if

## UPLINK OFFSETS
display "Sending offsets A: %4d" !LI1
display ", B: %4d" !LI2 to 24
display ", C: %4d" !LI3 to 33
queue on
tcblock mbltaofs 0
tcblock mbltbofs 0
tcblock mbltcofs 0
tcblock mbltaofs !LI1
tcblock mbltbofs !LI2
queue off
tcblock mbltcofs !LI3

## CONFIRM SUCCESS
menu !LS0 "Satisfied with pointing?" "YES, done!" "NO, redo it!"
if !LS0 = "NO, redo it!" then 
  goto center
end if

done:
writen !GS9 "Finished procedure move_mirror : %s" !TIME
writen !GS9 " "
display "Finished Procedure move_mirror                                   "
return
