[r4]: / trunk / SM3_Drive_RaspiStepper / SM3_Drive_RaspiStepper / Function Blocks / AXIS_REF_RaspiStepper / process IO data / PrepareOutputs / svnobj  Maximize  Restore  History

Download this file

30 lines (29 with data), 5.0 kB 

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
METHOD PrepareOutputs : BOOL
VAR
	{attribute 'noinit'}
	di: DINT; 
	//{attribute 'noinit'}
hes(bHigh:=bLimitPos, bLow:=bLimitNeg, Axis:=THIS^);
SMC_CheckSetPositionForResiduals(pDrive:=THIS);
fPosError := fLastSetPosition + (fLastOffset - fOffsetPosition) - fActPosition;
IF iMovementType = 0 THEN
	IF fPosError > fPositionPeriod/2 THEN
		fPosError := fPosError - fPositionPeriod;
	ELSIF fPosError < -fPositionPeriod/2 THEN
		fPosError := fPosError + fPositionPeriod;
	END_IF
	IF ABS(fPosError*fScalefactor) < 1 THEN
		fPosError := 0;
ELSIF NOT bError THEN
	IF byControllerMode = 2 THEN (* velocity controlled *)
		sc.lrFreq:= fSetVelocity * fFactorVel;
	ELSE (*position control *)
	IF fSetVelocity = 0 AND_THEN ABS(fPosError * fScalefactor) < 0.5 THEN
		sc.lrFreq := 0;
	sc.bRegulatorOn := bRegulatorOn;
ELSE
	sc.lrFreq := 0;
	sc.bRegulatorOn := FALSE;	
END_IF
fLastSetPosition := fSetPosition;
fLastOffset := fOffsetPosition;