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