Industrielle styresystemer - Wiki
FUNCTION_BLOCK FbStepperMotor
VAR_INPUT
powerON: BOOL;
endbreakPosDir: BOOL;
endbreakNegDir: BOOL;
setPos: DINT;
reset: BOOL;
start: BOOL;
iPort: WagoTypesModule_75x_67x.I_Module_75x_67x;
END_VAR
VAR_OUTPUT
actualPos: DINT;
resetReq: BOOL;
error: BOOL;
positionReached: BOOL;
oStatus: WagoSysErrorBase.FbResult;
END_VAR
VAR
xPower: FbPower;
xReadPos: FbReadActualPosition;
xMove: FbMoveAbsolute;
xStop: FbStop;
xReset: FbReset;
endbreakLimitPos: BOOL;
endbreakLimitNeg: BOOL;
PositiveDirSet: BOOL;
NegativeDirSet : BOOL;
stop : BOOL;
Pos : DINT;
END_VAR
Code Block | ||||
---|---|---|---|---|
| ||||
//Power on the module
xPower(xEnable:=powerON, I_Port:=iPort);
error:= xPower.xError;
//Stopping the servo drive when endbreaks activated
xStop(xExecute:= stop, I_Port:= iPort);
//Reset if fault on module
xReset(xExecute:= reset, I_Port:= iPort);
//Read data from module
xReadPos(xEnable:= powerON, I_Port:= iPort);
actualPos:= xReadPos.diActualPosition;
resetReq:= xReadPos.xResetRequired;
error:= xReadPos.xError;
oStatus:= xReadPos.oStatus;
//Move ablosute position of stepper
xMove(xExecute:= start, I_Port:= iPort, iSpeed:= 5000, wAcceleration:= 15000, diPosition:= Pos);
positionReached:= xMove.xDone;
//Endbreak positive direction
endbreakLimitPos:= endbreakPosDir;
IF endbreakLimitPos AND NOT PositiveDirSet THEN
Pos:= actualPos;
stop:= TRUE;
PositiveDirSet :=TRUE;
END_IF
IF PositiveDirSet AND (setPos < actualPos) THEN
Pos:= setPos;
stop:=FALSE;
IF NOT endbreakLimitPos THEN
PositiveDirSet:= FALSE;
END_IF
END_IF
//Endbreak negative direction
endbreakLimitNeg:= endbreakNegDir;
IF endbreakLimitNeg AND NOT NegativeDirSet THEN
Pos:= actualPos;
stop:= TRUE;
NegativeDirSet :=TRUE;
END_IF
IF NegativeDirSet AND (setPos > actualPos) THEN
Pos:= setPos;
stop:=FALSE;
IF NOT endbreakLimitNeg THEN
NegativeDirSet:= FALSE;
END_IF
END_IF
//Set pos = setpos if neither endbreaks are active
IF NOT endbreakLimitPos AND NOT endbreakLimitNeg THEN
pos:= setPos;
END_IF |