Option Explicit
' inputs:
' cmdInpL/R - CMD_xxx
' posInpL/R - position (0..600.0mm) (only used for CMD_POSITION)
' velInpL/R - velocity (1..100%)
'
' outputs:
' outStsL/R - STS_xxx
' outPosL/R - position (0..600.0mm) (for CMD_POSITION/_OPEN/_CLOSE)

' FSM commands
#define	CMD_STOP		0
#define	CMD_MOVE_POS    1
#define	CMD_MOVE_OUT    2
#define	CMD_MOVE_IN     3

' FSM states
#define	STATE_NONE	    0
#define STATE_CLEARING  1
#define	STATE_FAULT1    2
#define STATE_FAULT2    3
#define	STATE_MOVING    4
#define	STATE_STOPPED1  5
#define STATE_STOPPED2  6

' Status
#define	STS_OK			0
#define	STS_FAULT		1
#define	STS_EOS_REV		2
#define	STS_EOS_FWD		3

' User VAR offsets (1: 1..5, 2: 6..10)
#define	VAR_CMD_INP		1	' CMD_xxx
#define	VAR_POS_INP		2	' 0..600 mm
#define	VAR_VEL_INP		3	' 0..100 %
#define	VAR_STS_OUT		4	' STS_xxx
#define	VAR_POS_OUT		5	' 0..600 mm

' Control modes
#define	MMOD_OPEN_LOOP	0
#define	MMOD_CL_POS_REL	2

' Motor status flag bit masks
#define	MOTFLAG_AMPS_LIMIT		0x01
#define	MOTFLAG_MOTOR_STALLED	0x02
#define	MOTFLAG_LOOP_ERROR		0x04
#define	MOTFLAG_SAFETY_STOP		0x08
#define	MOTFLAG_FWD_LIMIT		0x10
#define	MOTFLAG_REV_LIMIT		0x20
#define	MOTFLAG_AMPS_TRIGGER	0x40
'
#define MOTFLAG_FAULT_HW		0x4E
#define	MOTFLAG_FAULT_SW		0x00

' input position limits
#define	POS_RNG_MIN	      0 ' mm
#define POS_INP_MIN       0 ' mm
#define POS_INP_MAX     200 ' mm
#define	POS_RNG_MAX	    200 ' mm


' input velocity limits
#define	VEL_RNG_MIN       0 ' %
#define VEL_INP_MIN       1 ' %
#define VEL_INP_MAX     100 ' %
#define	VEL_RNG_MAX	    100 ' %

' motor limits
#define	MOT_POS_MIN   -1000 ' counts (-1000)
#define	MOT_POS_MAX	   1000 ' counts (+1000)

#define	MOT_VEL_MIN	      1 ' RPM
#define	MOT_VEL_MAX     100 ' 65535 ' RPM

' target positions
#define	MOT_POS_CLOSED	MOT_POS_MIN
#define	MOT_POS_OPEN	MOT_POS_MAX

' declare variables
dim fltFlag     as integer
dim	faultHw		as boolean
dim faultSw		as boolean
dim cmdInp      as integer
dim	limitFwd	as boolean
dim limitRev	as boolean
dim motFlag     as integer
dim	motor       as integer
dim motPos      as integer
dim motVel      as integer
dim	moving[3]   as boolean
dim	posCur		as integer
dim	posInp		as integer
dim posOut      as integer
dim prvPos[3]   as integer
dim prvVel[3]   as integer
dim state[3]    as integer
dim status[3]   as integer
dim varBase     as integer
dim velInp      as integer

' initialize
setcommand(_EX, 1)	' emergency stop
For motor = 1 AndWhile motor <= 2 Evaluate motor += 1
    setconfig (_MMOD,  motor, MMOD_OPEN_LOOP)
    setcommand(_GO,    motor, 0)
    setconfig (_CLERD, motor, 0)    '*** set this to non-zero!
    state [motor] = STATE_STOPPED1
    status[motor] = STS_OK	
	prvPos[motor] = 9999
	prvVel[motor] = -1
	moving[motor] = false
Next
setcommand(_MG, 1)	' release stop

While True
    ' handle motors
    varBase = 0
    For motor = 1 AndWhile motor <= 2 Evaluate motor += 1
        ' get status
        fltFlag  = getvalue(_FLTFLAG, 1)
        motFlag  = getvalue(_MOTFLAG, motor)
        posCur   = getvalue(_FEEDBK,  motor)
        ' interpret status
 		faultSw  =  (motFlag and MOTFLAG_FAULT_SW ) <> 0
		faultHw  = ((motFlag and MOTFLAG_FAULT_HW ) <> 0) or  (fltFlag <> 0)
		limitFwd = ((motFlag and MOTFLAG_FWD_LIMIT) <> 0)
		limitRev = ((motFlag and MOTFLAG_REV_LIMIT) <> 0)
        ' get inputs
        cmdInp   = getvalue(_VAR, varBase + VAR_CMD_INP)
        posInp   = getvalue(_VAR, varBase + VAR_POS_INP)
        velInp   = getvalue(_VAR, varBase + VAR_VEL_INP)
        ' range-check
        if posInp < POS_INP_MIN then
           posInp = POS_INP_MIN
        elseif posInp > POS_INP_MAX then
           posInp = POS_INP_MAX
        end if
        if velInp < VEL_INP_MIN then
           velInp = VEL_INP_MIN
        elseif velInp > VEL_INP_MAX then
           velInp = VEL_INP_MAX
        end if

        ' handle states
		' stopped
		if (state[motor] = STATE_STOPPED1) then
			if (cmdInp = CMD_STOP) then
				state[motor] = STATE_STOPPED2
			end if
		elseif (state[motor] = STATE_STOPPED2)
			if (cmdInp = CMD_MOVE_IN) or (cmdInp = CMD_MOVE_OUT) or (cmdInp = CMD_MOVE_POS) then
				state[motor] = STATE_MOVING
			end if
		' fault
		elseif (state[motor] = STATE_FAULT1) then
			if (cmdInp = CMD_STOP) then
				state[motor] = STATE_FAULT2
			end if
		elseif (state[motor] = STATE_FAULT2) then
			if (cmdInp = CMD_MOVE_IN) or (cmdInp = CMD_MOVE_OUT) or (cmdInp = CMD_MOVE_POS) then
				' clear fault
                setconfig (_MMOD,  motor, MMOD_OPEN_LOOP)
                setcommand(_MSTOP, motor)
                setconfig (_MMOD,  motor, MMOD_CL_POS_REL)
                setcommand(_GO,    motor, getvalue(_FEEDBK, motor))
				' next state
				state[motor] = STATE_MOVING
			end if
		' moving
		elseif (state[motor] = STATE_MOVING)
			' check for faults
			if faultHw
				status[motor] = STS_FAULT
				state [motor] = STATE_FAULT1
			elseif faultSw
				gosub MotorStop
				status[motor] = STS_FAULT
				state [motor] = STATE_FAULT2
            elseif (cmdInp = CMD_MOVE_IN) or (cmdInp = CMD_MOVE_OUT) or (cmdInp = CMD_MOVE_POS) then
                ' determine next position
                if cmdInp = CMD_MOVE_IN then
                    motPos = MOT_POS_CLOSED
                elseif cmdInp = CMD_MOVE_OUT then
                    motPos = MOT_POS_OPEN
                else
                    motPos = MOT_POS_MIN + ((posInp - POS_RNG_MIN) * (MOT_POS_MAX - MOT_POS_MIN)) / (POS_RNG_MAX - POS_RNG_MIN)
                end if
               ' respect limits
                if limitFwd and (motPos >= posCur) then
                    gosub MotorStop
                    status[motor] = STS_EOS_FWD
                    state [motor] = STATE_STOPPED1
                elseif limitRev and (motPos <= posCur) then
                    gosub MotorStop
                    status[motor] = STS_EOS_REV
                    state [motor] = STATE_STOPPED1
                else
                    ' determine next velocity
                    motVel   = MOT_VEL_MIN + ((velInp - VEL_RNG_MIN) * (MOT_VEL_MAX - MOT_VEL_MIN)) / (VEL_RNG_MAX - VEL_RNG_MIN)
                    ' move
					if not moving[motor] then
                    	setconfig (_MMOD,   motor, MMOD_CL_POS_REL)
                        setcommand(_AC,  motor, 5000) '0.1 RPM
                        setcommand(_DC,  motor, 5000)
						setcommand(_GO,  motor, motPos)
						prvPos[motor] = motPos
						moving[motor] = true
					elseif prvPos[motor] <> motPos then
						setcommand(_GO,  motor, motPos)
						prvPos[motor] = motPos
					end if
					if prvVel[motor] <> motVel then
'						setcommand(_GO, motor, posCur)
						setcommand(_S, motor, motVel)
						prvVel[motor] = motVel
					end if
                    status[motor] = STS_OK
                end if
			else ' stop or anything unrecognized
				gosub MotorStop
				status[motor] = STS_OK
				state [motor] = STATE_STOPPED2
			end if
 		end if

        ' set outputs
        setcommand(_VAR, varBase + VAR_STS_OUT, status[motor])
        ' convert to user values
        posOut = POS_RNG_MIN + ((posCur - MOT_POS_MIN) * (POS_RNG_MAX - POS_RNG_MIN)) / (MOT_POS_MAX - MOT_POS_MIN)
        setcommand(_VAR, varBase + VAR_POS_OUT, posOut)

        ' next motor
        varBase += 5
    Next

    ' pause?
	wait(100)

End While

MotorStop:
	if true then
		' turn off motor power
	   	setconfig (_MMOD,  motor, MMOD_OPEN_LOOP)
		setcommand(_ACCEL, motor, 500000)
		setcommand(_DECEL, motor, 500000)
		setcommand(_MSTOP, motor)
	else
		' command to current position
		setconfig (_MMOD,  motor, MMOD_CL_POS_REL)
		setcommand(_ACCEL, motor, 500000)
		setcommand(_DECEL, motor, 500000)
		setcommand(_GO,	   motor, getvalue(_FEEDBK, motor))
	end if
	moving[motor] = false
	return