--- a
+++ b/trunk/SM3_Drive_RaspiStepper/SM3_Drive_RaspiStepper/Function Blocks/StepperControl/svnobj
@@ -0,0 +1,6 @@
+Gx_l
G!0…jüos0Êœá¼rRŽþSystem.StringL{6f9dac99-8de1-4efc-8465-68ac443b7d08}SpecialFuncL{0db3d7bb-cde0-4416-9a7b-ce49a0124323}NoneImplementationL{3b83b776-fb25-43b8-99f2-3c507c9143fc}TextDocument	L{f3878285-8e4f-490b-bb1b-9acbb7eb04db}
+TextLinesL{a5de0b0b-1cb5-4913-ac21-9d70293ec00d}Id
longTagText\IF hIecTask = SysTypes.RTS_INVALID_HANDLE THEN	RETURN;END_IFCASE eState OF4StepperControl_STATE.INIT://Set IO-Pinsd	IF eControlType = StepperControlType.DIRECT THEN  		IF iA > 0 THENT			GVL.GPIO.aeType[iA]      := GPIOType.O;		END_IF"		IF iA_ > 0 THENT			GVL.GPIO.aeType[iA_]     := GPIOType.O; 		IF iB > 0 THENT			GVL.GPIO.aeType[iB]      := GPIOType.O;"		IF iB_ > 0 THEN T			GVL.GPIO.aeType[iB_]     := GPIOType.O;!*		GVL.GPIO.SetType();"
+	ELSE#&		IF iPuls > 0 THEN$T			GVL.GPIO.aeType[iPuls]   := GPIOType.O;%$		IF iDir > 0 THEN&T			GVL.GPIO.aeType[iDir]    := GPIOType.O;',		IF iDisable > 0 THEN(V			GVL.GPIO.aeType[iDisable] := GPIOType.O;)*		IF iEnable > 0 THEN*T			GVL.GPIO.aeType[iEnable] := GPIOType.O;+*		xHalfStep := FALSE;,Z		gvl.GPIO.SetPulsDir(i := iDir, on := xDir);-	END_IF.	/"//Get Task-Handle0N	SysTask.SysTaskGetCurrent(ADR(hTask));1Z	IF hTask <> SysTypes.RTS_INVALID_HANDLE THEN2V		eState := StepperControl_STATE.OPERATION;3	t := LTIME();4@StepperControl_STATE.OPERATION:	5>// measure time since last call6	tAlt := t;7(	tCycle := t - tAlt;8(	IF lrFreq <> 0 THEN9.		IF lrMaxFreq > 0 THEN:			udiStep := TO_UDINT(1E6/ABS(LIMIT(-lrMaxFreq, lrFreq, lrMaxFreq)*2));;		ELSE<T			udiStep := TO_UDINT(1E6/ABS(lrFreq*2));=L		IF udiStep > udiMasterCycleTime THEN>j			//one step takes longer than the master cycle time?F			IF lrLastFreq * lrFreq < 0 THEN	@0				//direction invertedA.				udiSetTimeSum := 0;B			END_IFCH			udiSetTime := udiMasterCycleTime;D`			IF udiSetTimeSum + udiSetTime >= udiStep THENE$				xStep := TRUE;Fl				udiSetTimeSum := MAX(0, udiStep - udiSetTimeSum); G			ELSEHb				udiSetTimeSum := udiSetTimeSum + udiSetTime;	IP			//more than one step per master cycleJ2			udiSetTime := udiStep;K"			xStep := TRUE;L,			udiSetTimeSum := 0;MF		udiSetTime := udiMasterCycleTime;N*		udiSetTimeSum := 0;OH	IF udiOldSetTime <> udiSetTime THENP|		udiResult := SysTask.SysTaskSetInterval(hTask, udiSetTime); Q<		udiOldSetTime := udiSetTime;RvIF xStep AND eControlType = StepperControlType.PULSDIR THENSv	//es darf nur die steigende Flanke als Step gezählt werdenT(	xPuls := NOT xPuls;U 	ptSleepUs := 1;V	IF xPuls THENWB		IF NOT xDir AND lrFreq > 0 THENX 			xDir := TRUE;Y\			gvl.GPIO.SetPulsDir(i := iDir, on := xDir);ZP			SysTaskWaitSleepUs(hTask, ptSleepUs);[@		ELSIF xDir AND lrFreq < 0 THEN\"			xDir := FALSE;]^		gvl.GPIO.SetPulsDir(i := iPuls, on := xPuls);^		_:	IF xPuls AND lrFreq > 0 THEN`J		dwCounterStep := dwCounterStep + 1;a@	ELSIF xPuls AND lrFreq < 0 THENbJ		dwCounterStep := dwCounterStep - 1;c.	IF iDisable <> -1 THENdz		gvl.GPIO.SetPulsDir(i := iDisable, on := NOT bRegulatorOn);e	END_IF		f,	IF iEnable <> -1 THENgp		gvl.GPIO.SetPulsDir(i := iEnable, on := bRegulatorOn);h6	dwActPos := dwCounterStep;iEND_IF	j&		IF xHalfStep THENkp			bySet := OutMask[dwCounterStep MOD 8];				//half steplB			dwActPos := dwCounterStep / 2;mv			bySet := OutMask[(dwCounterStep MOD 4)*2];			//full stepn:			dwActPos := dwCounterStep;o$		IF iA <> -1 THENp|			GVL.GPIO.axOutputs[iA] 	:= xA 	:= bySet.0 AND bRegulatorOn;q&		IF iA_ <> -1 THENr~			GVL.GPIO.axOutputs[iA_] := xA_ 	:= bySet.1 AND bRegulatorOn;s$		IF iB <> -1 THENt|			GVL.GPIO.axOutputs[iB] 	:= xB 	:= bySet.2 AND bRegulatorOn;u&		IF iB_ <> -1 THENv~			GVL.GPIO.axOutputs[iB_] := xB_ 	:= bySet.3 AND bRegulatorOn;w0		GVL.GPIO.SetOutputs();xL		IF lrPower < 1 AND bRegulatorOn THENyf			//wait the time slice where lrPower shall be on.z`			stSleepUs := TO_ULINT(udiSetTime * lrPower); {P			SysTaskWaitSleepUs(hTask, stSleepUs);|*			ClearOutputPins();}		END_IF	~END_CASEInterface€L{a9ed5b7e-75c5-4651-af16-d2c27e98cb94}:FUNCTION_BLOCK StepperControl‚VAR_INPUTƒ¤	eControlType:StepperControlType;//control type : direct (A, A_,B, B_) or puls/dir„z	lrFreq: LREAL;					//steps per second (positive or negative)…”	lrMaxFreq: LREAL;				//maximum steps per seond that the motor can follow	†„	udiMasterCycleTime: UDINT;		//cycle time of commanding task in us‡ì	lrPower: LREAL;					//[0..1] fraction of the cycle, where outputs are set High; determines the power of the motor				ˆz	iA, iA_, iB, iB_ : INT := -1;	//GPIO pins for direct control‰t	iPuls, iDir: INT :=-1;			//GPIO pins for puls/dir controlŠv	iDisable, iEnable: INT :=-1;	//GPIO pins for power control‹ˆ	xHalfStep: BOOL;				//Motor takes half steps not full steps if TrueŒ\	bRegulatorOn: BOOL;				//bRegulatorOn of AxisÊ	strTaskGroup: STRING := 'IEC-Tasks';	//name of the task group (only relevant for multi core systems)ŽEND_VARVAR_OUTPUT	tCycle: LTIME;‘|	dwActPos: DWORD;				//set position of motor (generated steps)’VAR“ //state variable”v	eState: StepperControl_STATE := StepperControl_STATE.INIT;•4//number of internal steps–,	dwCounterStep: DWORD;—//Task-Handles˜„	hIecTask: SysTypes.RTS_IEC_HANDLE := SysTypes.RTS_INVALID_HANDLE;™~	hTask: SysTypes.RTS_IEC_HANDLE := SysTypes.RTS_INVALID_HANDLE;š,//mask for output bits›Ä	OutMask: ARRAY[0..7] OF BYTE := [2#1000, 2#1010, 2#0010, 2#0110, 2#0100, 2#0101, 2#0001, 2#1001];œ6//current Byte of the Array	bySet: BYTE;ž$//GPIO-AssignmentsŸ0	xA, xA_, xB, xB_: BOOL; 8	xPuls ,xDir : BOOL := TRUE;¡,//Cycletime monitoring¢	t: LTIME;£H//time in which the ouputs are set.	¤(	stSleepUs: SYSTIME;¥(	ptSleepUs: SYSTIME;¦//step time § 	udiStep: UDINT;¨L//set time for the task and last value©D	udiSetTime, udiOldSetTime: UDINT;ªØ// accumulated time without step (used for low frequencies, where step time is larger than master cycle time«,	udiSetTimeSum: UDINT;¬2// last cycle's frequency­&	lrLastFreq: LREAL;®.// task info variables	¯>	pApp : POINTER TO APPLICATION;°X    TaskInfo : Task_Info2 := (dwVersion:=2);±X	sTaskName: STRING := 'Task_StepperControl';²2// instance number (0..n)³&	uiInstance : UINT;´VAR_STATµŽ// static instance number; helps to find the instance number of this FB¶2	uiInstanceCounter: UINT;·VAR_TEMP¸6	udiResult: RTS_IEC_RESULT;¹	tAlt: LTIME;º	xStep: BOOL;»"UniqueIdGenerator¼2056½POULevel¾L{8e575c5b-1d37-49c6-941b-5c0ec7874787}¿StandardÀ ChildObjectGuidsÁ8System.Collections.ArrayListÂ,AddAttributeSubsequentÃboolÐÐ-ÐÐ	E
+ŸÐÐ
­XÚÐ-ÐÐ
­YÚÐ-ÐÐ
­ZÚÐ-ÐÐ
­[ÚÐ-ÐÐ
­\ÚÐ-ÐÐ
­]ÚÐ-ÐÐ
­^ÚÐ-ÐÐ
­_ÚÐ-ÐÐ
­`ÚÐ-ÐÐ
­aÚÐ-ÐÐ
­bÚÐ-ÐÐ
­cÚÐ-ÐÐ
­dÚÐ-ÐÐ
­eÚÐ-ÐÐ
­fÚÐ-ÐÐ
­gÚÐ-ÐÐ
­hÚÐ-ÐÐ
­iÚÐ-ÐÐ
­jÚÐ- ÐÐ
­kÚÐ-ÐÐ
­ÚÐ-!ÐÐ
­lÚÐ-"ÐÐ
­mÚÐ-#ÐÐ
­nÚÐ-$ÐÐ
­oÚÐ-ÐÐ
­pÚÐ-%ÐÐ
­qÚÐ-&ÐÐ
­rÚÐ-ÐÐ
­sÚÐ-'ÐÐ
­tÚÐ-(ÐÐ
­uÚÐ-ÐÐ
­vÚÐ-)ÐÐ
­wÚÐ-*ÐÐ
­xÚÐ-ÐÐ
­ÚÐ-!ÐÐ
­yÚÐ-+ÐÐ
­ÚÐ-,ÐÐ
­zÚÐ--ÐÐ
­{ÚÐ-.ÐÐ
­}ÚÐ-/ÐÐ
­~ÚÐ-0ÐÐ
­ÚÐ-1ÐÐ
­€ÚÐ-2ÐÐ
­ÚÐ--ÐÐ
­‚ÚÐ-3ÐÐ
­ƒÚÐ-ÐÐ
­„ÚÐ-4ÐÐ
­…ÚÐ-ÐÐ
­†ÚÐ-.ÐÐ
­‡ÚÐ-5ÐÐ
­ˆÚÐ-6ÐÐ
­‰ÚÐ-3ÐÐ
­ŠÚÐ-7ÐÐ
­‹ÚÐ-.ÐÐ
­ŒÚÐ-8ÐÐ
­ÚÐ-9ÐÐ
­ŽÚÐ-:ÐÐ
­ÚÐ-;ÐÐ
­ÚÐ-<ÐÐ
­‘ÚÐ-ÐÐ
­’ÚÐ-=ÐÐ
­“ÚÐ->ÐÐ
­”ÚÐ-?ÐÐ
­•ÚÐ-@ÐÐ
­–ÚÐ-AÐÐ
­—ÚÐ-BÐÐ
­˜ÚÐ-CÐÐ
­™ÚÐ-DÐÐ
­šÚÐ-EÐÐ
­›ÚÐ-FÐÐ
­œÚÐ-GÐÐ
­ÚÐ-HÐÐ
­žÚÐ-BÐÐ
­ŸÚÐ-;ÐÐ
­ ÚÐ-IÐÐ
­¡ÚÐ-JÐÐ
­¢ÚÐ-KÐÐ
­£ÚÐ-LÐÐ
­¤ÚÐ-ÐÐ
­¥ÚÐ-"ÐÐ
­¦ÚÐ-MÐÐ
­§ÚÐ-NÐÐ
­¨ÚÐ--ÐÐ
­©ÚÐ-.ÐÐ
­ªÚÐ-OÐÐ
­«ÚÐ-PÐÐ
­¬ÚÐ-QÐÐ
­­ÚÐ--ÐÐ
­®ÚÐ-.ÐÐ
­¯ÚÐ-RÐÐ
­°ÚÐ-SÐÐ
­±ÚÐ-TÐÐ
­²ÚÐ-UÐÐ
­³ÚÐ-.ÐÐ
­´ÚÐ-VÐÐ
­µÚÐ-WÐÐ
­¶ÚÐ-XÐÐ
­·ÚÐ-YÐÐ
­¸ÚÐ-ZÐÐ
­¹ÚÐ-[ÐÐ
­ºÚÐ-\ÐÐ
­»ÚÐ-YÐÐ
­¼ÚÐ-ZÐÐ
­½ÚÐ-ÐÐ
­¾ÚÐ-]ÐÐ
­¿ÚÐ-^ÐÐ
­ÀÚÐ-"ÐÐ
­ÁÚÐ-]ÐÐ
­ÂÚÐ--ÐÐ
­ÃÚÐ-.ÐÐ
­ÄÚÐ-_ÐÐ
­ÅÚÐ-`ÐÐ
­ÆÚÐ-aÐÐ
­ÇÚÐ-bÐÐ
­ÈÚÐ--ÐÐ
­öÚÐ-.ÐÐ
­ùÚÐ-cÐÐ
­ÚÐ-dÐÐ
­ûÚÐ-eÐÐ
­üÚÐ-fÐÐ
­ÚÐ-gÐÐ
­þÚÐ-eÐÐ
­ÿÚÐ-.ÐÐ
­÷ÚÐ-hÐÐ
­ÉÚÐ-iÐÐ
­ÊÚÐ-ÐÐ
­ËÚÐ-ÐÐ
­ÌÚÐ-jÐÐ
­ÍÚÐ-kÐÐ
­ÎÚÐ-lÐÐ
­ÏÚÐ-;ÐÐ
­ÐÚÐ-mÐÐ
­ÑÚÐ-nÐÐ
­ÒÚÐ-ÐÐ
­ÓÚÐ-.ÐÐ
­ÔÚÐ-oÐÐ
­ÕÚÐ-pÐÐ
­ÖÚÐ-ÐÐ
­×ÚÐ-qÐÐ
­ØÚÐ-rÐÐ
­ÙÚÐ-ÐÐ
­ÚÚÐ-sÐÐ
­ÛÚÐ-tÐÐ
­ÜÚÐ-ÐÐ
­ÝÚÐ-uÐÐ
­ÞÚÐ-vÐÐ
­ßÚÐ-ÐÐ
­àÚÐ-wÐÐ
­áÚÐ-^ÐÐ
­âÚÐ-xÐÐ
­ãÚÐ-yÐÐ
­äÚÐ-zÐÐ
­åÚÐ-{ÐÐ
­æÚÐ-^ÐÐ
­çÚÐ-|ÐÐ
­èÚÐ-}ÐÐ
­óÚÐ--ÐÐ
­õÚÐ-~ÐÐ
­žÚÐ-ЀÐ	E
+KÐÐ
­ÝÚÐ-ÐÐ
­ÞÚÐ-‚ÐÐ
­4ÚÐ-.ÐÐ
­ßÚÐ-ƒÐÐ
­àÚÐ-„ÐÐ
­áÚÐ-…ÐÐ
­âÚÐ-†ÐÐ
­ãÚÐ-‡ÐÐ
­äÚÐ-ˆÐÐ
­åÚÐ-‰ÐÐ
­æÚÐ-ŠÐÐ
­çÚÐ-‹ÐÐ
­èÚÐ-ŒÐÐ
­éÚÐ-ÐÐ
­êÚÐ-ŽÐÐ
­ëÚÐ-ÐÐ
­ìÚÐ-ÐÐ
­íÚÐ-‘ÐÐ
­îÚÐ-ŽÐÐ
­ïÚÐ-’ÐÐ
­ðÚÐ-“ÐÐ
­ñÚÐ-”ÐÐ
­òÚÐ-.ÐÐ
­óÚÐ-•ÐÐ
­ôÚÐ-–ÐÐ
­õÚÐ-.ÐÐ
­öÚÐ-—ÐÐ
­÷ÚÐ-˜ÐÐ
­øÚÐ-™ÐÐ
­ùÚÐ-ÐÐ
­úÚÐ-šÐÐ
­ûÚÐ-›ÐÐ
­üÚÐ-ÐÐ
­ýÚÐ-œÐÐ
­þÚÐ-ÐÐ
­ÿÚÐ-žÐÐ
­ÚÐ-ŸÐÐ
­ÚÐ- ÐÐ
­ÚÐ-ÐÐ
­ÚÐ-¡ÐÐ
­ÚÐ-¢ÐÐ
­ÚÐ-ÐÐ
­ÚÐ-£ÐÐ
­ÚÐ-¤ÐÐ
­VÚÐ-¥ÐÐ
­	ÚÐ-ÐÐ
­
+ÚÐ-¦ÐÐ
­ÚÐ-§ÐÐ
­ÚÐ-.ÐÐ
­
ÚÐ-¨ÐÐ
­ÚÐ-©ÐÐ
­ÚÐ-.ÐÐ
­ÚÐ-ªÐÐ
­ÚÐ-«ÐÐ
­ÚÐ-ÐÐ
­ÚÐ-¬ÐÐ
­ÚÐ-­ÐÐ
­ÚÐ-.ÐÐ
­ÚÐ-®ÐÐ
­ÚÐ-¯ÐÐ
­ÚÐ-°ÐÐ
­ÚÐ-±ÐÐ
­ÚÐ-.ÐÐ
­ÚÐ-²ÐÐ
­ÚÐ-³ÐÐ
­ÚÐ-ŽÐÐ
­ÚÐ-´ÐÐ
­ ÚÐ-µÐÐ
­!ÚÐ-¶ÐÐ
­"ÚÐ-ŽÐÐ
­#ÚÐ-·ÐÐ
­$ÚÐ-¸ÐÐ
­%ÚÐ-¹ÐÐ
­&ÚÐ-ºÐÐ
­™ÚÐ-Žл-¼н¾-¿ÀÁÐÂí
\ No newline at end of file