//////////////////////////////////////////////////////////////////////////////////////////////////////// // Automatic offset correction for a rotative absolute axis which can move endless // dwFBUnwind, bFBUnwind, bDriveDir, dwGearRatio can be adjusted to the application // // Version When Who What // --------------------------------------------- // V 0.003 20.05.15 MRupf KM detected revolutions up to Gear ratio with one shot (for high speed) // V 0.002 19.05.15 MRupf KM FBunwind can be +- or >0 (switched in #13) // Revolution detection works also with drv.dir 1 // V 0.001 05.05.15 MRupf KM First version ////////////////////////////////////////////////////////////////////////////////////////////////////////// // init values for all the constants #dwFBUnwind := 4096; // maximum number of revolution the feedback unit can indicate #bFBUnwind := FALSE; // TRUE = Feedback revolutions are 0-dwFbUnwind, FALSE = Feedback unwind are +-dwFBUnwind/2 #bDriveDir := TRUE; // Direction of a positive feedback value #dwGearRatio := 10; // gear ratio between application and motor revolution (has to be set as the modulo in the AKD) #dwPosResolution := 1048576; // Modbus resolution standard is 2^20. !! if this has been changed some more modifications are needed !! #dwFaultPerUnw := ((#dwGearRatio - ((#dwFBUnwind / 2) MOD #dwGearRatio)) * 2 * #dwPosResolution) MOD (#dwGearRatio * #dwPosResolution); // init a new execution of the reference procedure IF #iReDoRef AND NOT #bStartDone THEN // Reset offset wiht stored values #bStartDone := TRUE; #bDoNewRefPos := FALSE; #wAutoOffsetStatus := 10; ELSIF #iSetRef AND NOT #bManualHomingDone THEN // Set new offset #bManualHomingDone := TRUE; #bDoNewRefPos := TRUE; #wAutoOffsetStatus := 10; END_IF; CASE #wAutoOffsetStatus OF 0: // Waiting for first start #oDone := FALSE; 10: // Read FB1.P #dwStartNumRev := 0; #oDone := FALSE; #oMBReq := TRUE; #oMBWrite := FALSE; #oMBLen := 4; #oMBAddr := 1610; // FB1.P #bMBCommStarted := TRUE; #wAutoOffsetStatus := 11; 11: // Wait for FB1Read done IF NOT #bMBCommStarted AND #iMBCommDone THEN #oMBReq := FALSE; #wAutoOffsetStatus := 20; END_IF; 20: // Calculate new reference offset #dwStartNumRev := SHR_DWORD(IN := #ioMBData1, N := 20) OR SHL_DWORD(IN := #ioMBData0, N := 12); #dwActPos := ((#dwStartNumRev MOD #dwGearRatio) * #dwPosResolution + (#ioMBData1 AND 16#000F_FFFF)) MOD (#dwGearRatio * #dwPosResolution); IF #bDriveDir THEN #dwActPos := (#dwPosResolution * #dwGearRatio) - #dwActPos; END_IF; IF #dwActPos < 0 THEN #dwActPos := #dwActPos + (#dwGearRatio * #dwPosResolution); END_IF; // Calculate the number of revolution done IF #bFBUnwind THEN IF #dwActNumRev >= 0 THEN #dwNumberOfUnw := #dwActNumRev / #dwFBUnwind; ELSE #dwNumberOfUnw := #dwActNumRev / #dwFBUnwind - 1; END_IF; ELSE IF #dwActNumRev > (#dwFBUnwind / -2) THEN #dwNumberOfUnw := (#dwActNumRev + #dwFBUnwind / 2) / (#dwFBUnwind); ELSE #dwNumberOfUnw := (#dwActNumRev + 1 + #dwFBUnwind / 2) / (#dwFBUnwind) - 1; END_IF; END_IF; // Set the corresponding reference positon IF #bDoNewRefPos THEN #dwNewRefpos1 := #iRefOffset - #dwActPos; ELSE IF #bDriveDir THEN #dwNewRefpos1 := (#dwNewRefpos1 + #dwNumberOfUnw * #dwFaultPerUnw) MOD (#dwGearRatio * #dwPosResolution); ELSE #dwNewRefpos1 := (#dwNewRefpos1 - #dwNumberOfUnw * #dwFaultPerUnw) MOD (#dwGearRatio * #dwPosResolution); END_IF; END_IF; IF #dwNewRefpos1.%X31 THEN // set the first 32 Bit to either get a minus or plus value #dwNewRefPos0 := -1; ELSE #dwNewRefPos0 := 0; END_IF; #wAutoOffsetStatus := 30; 30: // Activate new reference position #oMBReq := TRUE; #oMBWrite := TRUE; #oMBLen := 4; #oMBAddr := 290; // FB1.OFFSET #ioMBData0 := #dwNewRefPos0; #ioMBData1 := #dwNewRefpos1; #bMBCommStarted := TRUE; #wAutoOffsetStatus := 31; 31: // If new reference position is set start normal run state #dwActNumRev := #dwStartNumRev; #dwOldActPos := #iActPos - #dwNewRefpos1 + #dwPosResolution * #dwGearRatio; // calculate the actual application position of the feedback #dwOldActRev := SHR_DWORD(IN := #dwActPos, N := 20) MOD #dwGearRatio; // read the actual unwind of the application #dwOldActPos := #dwActPos MOD #dwPosResolution; // calculated the feedback position from the application position // If the drives positive move is in negative feedback direction all values have to be inverted IF #bDriveDir THEN #dwOldActPos := - #dwOldActPos; #dwOldActRev := - #dwOldActRev; END_IF; IF #iMBCommDone AND NOT #bMBCommStarted THEN #oMBReq := FALSE; #wAutoOffsetStatus := 40; END_IF; 40: // count revolutions (normal run state) #dwActPos := #iActPos - #dwNewRefpos1 + #dwPosResolution * #dwGearRatio; // calculate the actual application position of the feedback #dwActRev := SHR_DWORD(IN := #dwActPos, N := 20) MOD #dwGearRatio; // read the actual unwind of the application #dwActPos := #dwActPos MOD #dwPosResolution; // calculated the feedback position from the application position #dwActVel := #iActVel; // If the drives positive move is in negative feedback direction all values have to be inverted IF #bDriveDir THEN #dwActPos := - #dwActPos; #dwActVel := - #iActVel; #dwActRev := - #dwActRev; END_IF; IF #dwActVel > 0 THEN // with positive movement the revolution have to be counted up IF #dwActVel > 16#0030_0000 AND (#dwOldActPos - #dwActPos) > 0 OR // for high speed only roll over with no window gets supervised #dwActVel < 16#0030_0000 AND (#dwOldActPos - #dwActPos) > (#dwPosResolution / 2) THEN // on low speed the position shift has to be 1/2 revolution #dwActNumRev := #dwActNumRev + (#dwActRev - #dwOldActRev + #dwGearRatio) MOD #dwGearRatio; // ++ number of revolution since last shift #dwOldActRev := #dwActRev; END_IF; ELSE // with negative movement the revolution have to be counted up IF #dwActVel < 16#FFD0_0000 AND (#dwActPos - #dwOldActPos) > 0 OR // for high speed only roll over with no window gets supervised #dwActVel > 16#FFD0_0000 AND (#dwActPos - #dwOldActPos) > (#dwPosResolution / 2) THEN // on low speed the position shift has to be 1/2 revolution #dwActNumRev := #dwActNumRev + (#dwActRev - #dwOldActRev - #dwGearRatio) MOD #dwGearRatio; // ++ number of revolution since last shift #dwOldActRev := #dwActRev; END_IF; END_IF; #dwOldActPos := #dwActPos; // Set new position value to be ready for the next cycle #oDone := TRUE; ELSE #oDone := FALSE; END_CASE; // Reset of the rising edges for all the input signals IF NOT #iReDoRef AND #bStartDone THEN #bStartDone := FALSE; END_IF; IF NOT #iMBCommDone AND #bMBCommStarted THEN #bMBCommStarted := FALSE; END_IF; IF NOT #iSetRef AND #bManualHomingDone THEN #bManualHomingDone := FALSE; END_IF;