########################################################################
### FILE:	stage.mod
### PURPOSE:	Stage control
### SCOPE:	Top Level
###
### Copyrights (C) 2010 Life Technologies.  All rights reserved.
########################################################################


### Create and populate STAGe branch
    DEFINT -hidden STAGe+ $Branch$ $RegBranch$ <quote.addstage>
        ### Create branch
        BRANch+ -replaceExisting -type=MotionControl $Branch$

	### Load stage control module
	$Branch$:MODule aliases.mod
	$Branch$:MODule stagecontrol.mod

        ### TODO: Get addresses independently of MCB branch!
        $RegBranch$:PROF:BASE?
        $Branch$:PROFile:BASE= $1 $2

        ### Stage axes
        $RegBranch$:MOT:X:BASE?
        $Branch$:AXIS+ X $1 $2
        $Branch$:X:SCALE= -real 10.0
        $Branch$:X:PARK= -absolute -steps $(MCB:MOTion:X:InitialPOSition?)
        $Branch$:X:SLACK= -steps $(MCB:MOTion:X:VerifyPositionTolerance?)

        $RegBranch$:MOT:Y:BASE?
        $Branch$:AXIS+ Y $1 $2
        $Branch$:Y:SCALE= -real 10.0
        $Branch$:Y:PARK= -absolute -steps 1000
        $Branch$:Y:SLACK= -steps $(MCB:MOTion:Y:VerifyPositionTolerance?)

        ### Objective
        $RegBranch$:MOT:Z:BASE?
        $Branch$:AXIS+ Z $1 $2
        $Branch$:Z:SCALE= -real 100.0
        $Branch$:Z:PARK= -absolute -steps 5000
        $Branch$:Z:SLACK= -steps 500

        ### Filter wheel
        $RegBranch$:MOT:F:BASE?
        $Branch$:AXIS+ F $1 $2 -type=Rotational
        $Branch$:F:StepsPerRevolution 80000
        $Branch$:F:SCALE= 16000
        $Branch$:F:PARK= 4
        $Branch$:F:SAFE= True
        $Branch$:PositionMapConvert F

        ### Shutter
        $RegBranch$:MOTion:S:BASE?
        $Branch$:AXIS+ S $1 $2 -type=Rotational
        $Branch$:S:StepsPerRevolution 50000
        $Branch$:S:SCALe= 12000
        $Branch$:S:PARK= 2
        $Branch$:S:SAFE= True
        $Branch$:PositionMapConvert S

        ### Autofocus axis
        $RegBranch$:FOC:BASE?
        $Branch$:AXIS+ PSD $1 $2 -type=Autofocus
        $Branch$:PSD:FocusAxis= Z

        ### Define common area subject to reservations
        STAGe:REServation+ InjectionPorts -X -Y -Z

	### If we are running a tethered setup, check for motion bounds
	IF $(SYSTem:ENVironment? -default=false TETHERED) <quote>
             STAGe:TetherCheck
        </quote>
    </quote.addstage>


### Remove STAGe branch
    DEFINT -hidden STAGe- $Branch$ <quote.delstage>
        BRANch- "$Branch$"
    </quote.delstage>


### Create STAGe: branch once MCB exists
    STATe:NEW Stage:Created -requires=mcb
    STATe:TRIGGer -states=Stage:Created -edge=Set    CreateStage "STAGe+ STAGe MCB"
    STATe:TRIGGer -states=Stage:Created -edge=Clear  DeleteStage "STAGe- STAGe"


### Disable motion  once front door is opened
    STATE:NEW Stage:Disabled -requires=Stage:Created -conflicts=FrontDoor:Locked
    STATE:TRIGGer+ -states=Stage:Disabled -edge=Set Stage:Disable <quote>
        STAGE:DISABLE
        STAGE:OFF
    </quote>

    STATE:TRIGGer+ -states=Stage:Disabled -edge=Clear Stage:Enable <quote>
        STAGE:OFF
        STAGE:ON
        STAGE:ENABLE
    </quote>

### Home Stage once created
    STATe:NEW Stage:Homed -requires=Stage:Created,Motion:Allowed -conflicts=Stage:Disabled
    STATe:TRIGger -states=Stage:Homed -edge=Set HomeStage <quote.home>
        UNLESS $(STAGE:HOMED?) <quote.needshome>
            SET tln $(STAGE:Z:REGisters:TravelLimitNegative?)
            TRY <quote.try>
               STAGE:Z:POSition -steps -absolute ${tln}
            </quote.try> <quote.except>
               STAGE:Z:StatusBits? RevLimitSwitch
               UNLess $1 "ERRor -id=$error$ -axis=STAGE:Z $arguments$ '$message$'"
            </quote.except>

            STAGE:HOME X Y
            STAGe:HOME Z F S
        </quote.needshome>
    </quote.home>


### Once homed, park - then we are ready
    STATe:NEW Stage:Ready -requires=Stage:Homed
    STATe:TRIGger -states=Stage:Ready -edge=Set ParkStage <quote>
        STAGe:PARK
    </quote>


### Set initialized state according to stage state
    STATe:DEPendency+ Imaging:Ready -requires=Stage:Ready
    STATe:DEPendency+ Instrument:Initialized -requires=Stage:Ready