SEW伺服电机IPOS典型控制程序.doc

/* - 10.18.01 D.Broerman Copied from Version 5 of Rack Stacker Software 08.01.02 D.Broerman Added Position_6a to control the Vaccum Cup extend position 10.08.02 D.Broerman V04 Added sequence to Load from Rack 31.01.03 St. Reuter V05 Communication via Device Net 17.04.03 IBB China CSG Pos.13 Necessary Parameters: - Operating Mode = VFC-n-Reg. & IPOS Parameter P700 Parameter P100 Sollwertquelle = Feldbus Parameter P101 Steuerquelle = Feldbus Binary Inputs Base Unit DI00 Enable P600 DI01 Release/Rapid Stop P601 DI02 High Speed Input 1 P602 DI03 High Speed Input 2 P603 DI04 Spare P604 DI05 Spare aaa Binary Option Card 1 P610 DI10 IPOS Input (Vacuum Cup Extended Prox) P611 DI11 IPOS Input (Vacuum Cup Retracted Prox) P612 DI12 IPOS Input (Vacuum Switch) P613 DI13 IPOS Input (Spare) P614 DI14 IPOS Input (Spare) P615 DI15 IPOS Input (Spare) P616 DI16 IPOS Input (Spare) P617 DI17 IPOS Input (Spare)Binary Inputs IPOS Inputs PA 1 (Device Net) DI20 IPOS Input (Start Cycle Signal Stacking) DI21 IPOS Input (Stop Cycle Signal) DI22 IPOS Input (MotorRight/ Halt, Arm Up Hand) DI23 IPOS Input (MotorLeft/ Halt, Arm Down Hand) DI24 IPOS Input (Vacuum Man Mode) DI25 IPOS Input (Release Man Mode) DI26 IPOS Input (Vacuum Cup In/Out Man Mode) DI27 IPOS Input (Stacker Mode) (Automatic = 1) (ManualMode = 0) DI30 IPOS Input (Loading Cycle Start) DI31 IPOS Input (Start Rack Reference) DI32 Spare DI33 Spare DI34 Spare DI35 Spare DI36 Spare DI37 Spare Binary Outputs Base Unit DB00 Configured with Brake P620 DO01 Drive Ready P621 DO02 Drive Fault Binary Output Option Card 1 P630 DO10 IPOS Output (Arm Down) P631 DO11 IPOS Output (Conv Release In Transport) P632 DO12 IPOS Output (Stacker Fault) P633 DO13 IPOS Output (Basic Position) P634 DO14 IPOS Output (Conv Release Out Transport) P635 DO15 IPOS Output (Vacuum Cup Extended) P636 DO16 IPOS Output (Rack Reference Position) P637 DO17 IPOS Output (Vacuum Fault) Output Option Card 2 DO20 IPOS Output (Vacuum Cup Extend Sol) DO21 IPOS Output (Vacuum Cup Retract Sol) DO22 IPOS Output (Vacuum On Sol) D023 IPOS Output (Vacuum Off Sol) DO24 IPOS Output (Blow Off On) DO25 IPOS Output (Spare) DO26 IPOS Output (Spare) DO27 IPOS Output (Spare) P941 IPOS Geberquelle = Motor Encoder on Terminal X15 Anwenderprogramm mit folgenden Funktionen : - 1)Allgemeiner ManualMode eines positioniergeregelten Antriebs. - 黚er zwei Eingangsklemmen kann der Antrieb mit einer festen Geschwindigkeit in beide Richtungen gefahren werden. - Die Lageregelung ist dabei aktiv. - Eine vorherige Referenzfahrt ist nicht notwendig. 2)Betriebsart Automatic*/#include #include /= Fahr Variables =#pragma globals 200 250long V_MotorRight, V_MotorLeft;/= Ziel Position - Variables =#define Position_0 H0#define Position_1 H1#define Position_2 H2#define Position_3 H3#define Position_4 H4#define Position_5 H5#define Position_6 H6#define Position_7 H7#define Position_8 H8#define Position_9 H9#define Position_10 H10#define Ref_Pos H11#define Position_6a H12#define Auto_Speed_0 H20#define Auto_Speed_1 H21#define Auto_Speed_2 H22#define Ref_Speed_0 H23#define Auto_Speed_3 H24#define Man_Speed_0 H25#define Auto_Speed_4 H26#define Auto_Speed_5 H27#define Auto_Speed_6 H28#define Position_Window H30#define M_State H31#define M_Fault H32#define M_Merker H33#define Pos_0 H35#define M_Loading H36#define M_Stacking H37#define Load_Pos_0 H40#define Load_Pos_1 H41#define Load_Pos_2 H42#define Load_Pos_3 H43#define Load_Pos_4 H44#define Load_Pos_5 H45#define Load_Pos_6 H46#define Load_Pos_7 H47#define Load_Pos_8 H48#define Load_Pos_9 H49#define Load_Pos_10 H50#define Load_Pos_11 H51#define Load_Pos_12 H52#define Load_Pos_6a H53#define Bustyp H100#define Le_PA_DATA H101#define PA1 H102#define PA2 H103#define PA3 H104#define Le_PE_DATA H110#define PE1 H111#define PE2 H112#define PE3 H113/= INPUTS =/Onboard Inputs#define E_DriveEnable DI00#define E_DriveRel DI01#define E_Touchp_1 DI02#define E_Touchp_2 DI03#define E_Spare_1 DI04#define E_Spare_2 DI05/Inputs Card DIO 11A#define E_CupExtendedProx DI10#define E_CupRetractedProx DI11#define E_VacuumSwitch DI12#define E_Spare_3 DI13#define E_Spare_4 DI14#define E_Spare_5 DI15#define E_Spare_6 DI16#define E_Spare_7 DI17/Inputs Field Bus#define E_Start_Cycle (PA1 & 0b1)!=0)#define E_Stop_Cycle (PA1 & 0b10)!=0)#define E_ArmRight (PA1 & 0b100)!=0)#define E_ArmLeft (PA1 & 0b1000)!=0)#define E_VacManMode (PA1 & 0b10000)!=0)#define E_RelManMode (PA1 & 0b100000)!=0)#define E_CupInOutManMode (PA1 & 0b1000000)!=0)#define E_AutoMode (PA1 & 0b10000000)!=0)#define E_LoadCycleStart (PA1 & 0b100000000)!=0)#define E_ReferenceRack (PA1 & 0b1000000000)!=0)#define E_Continue_Cycle (PA1 & 0b10000000000)!=0)#define E_Reset_Cycle (PA1 & 0b100000000000)!=0)#define E_Resume_Cycle (PA1 & 0b1000000000000)!=0)#define E_Vacuum_On_Auto (PA1 & 0b10000000000000)!=0)/= OUTPUTS =/Onboard Outputs#define A_DriveReady DO01#define A_DriveFault DO02/Outputs Card DIO 11A#define A_CupExtendSol 0 /DO20#define A_CupRetractSol 1 /DO21#define A_VacuumOnSol 2 /DO22#define A_VacuumOffSol 3 /DO23#define A_BlowOffSol 4 /DO24/Outputs Fieldbus#define PE1_Output PE1#define A_StackerArmDown 0#define A_ConvRel 1 /(PE1 & 0b10)!=0)#define A_StackerFault 2 /(PE1 & 0b100)!=0)#define A_HomePosition 3 /(PE1 & 0b1000)!=0)#define A_ConvRelOut 4 /(PE1 & 0b10000)!=0)#define A_CupExtendSwitch 5 /(PE1 & 0b100000)!=0)#define A_ArmReferencePos 6 /(PE1 & 0b1000000)!=0)#define A_VacuumFault 7 /(PE1 & 0b10000000)!=0)#define A_ArmProbePos 8 /(PE1 & 0b100000000)!=0)#define A_CupRetractSwitch 9 /(PE1 & 0b1000000000)!=0)#define A_VacuumConfirm 10 /(PE1 & 0b10000000000)!=0)#define A_RelCrosstrREV 11 /(PE1 & 0b100000000000)!=0)/= Festwerte =#pragma list/*= Subroutines=*/*= Subroutine for Initializing the System.=*/Initialize() _AxisStop( AS_RSTOP ); OptOutpIPOS = 0; / all Outputs Off ControlWord = 0; / IPOS Control Word Off Pos_0 = 1000; Auto_Speed_0 = 15000; / Touch Probe Searching Auto_Speed_1 = 19500; / Drive Down Stacking Auto_Speed_2 = 5000; Auto_Speed_3 = 10000; Auto_Speed_4 = 15000; / Drive UP Stacking Auto_Speed_5 = 1500; / Med Peel off speed Auto_Speed_6 = 100; / Slow Peel off speed Ref_Speed_0 = 29000; Man_Speed_0 = 5000; Position_1 = 250000; M_State = 0; M_Fault = 0; M_Loading = 0; M_Stacking = 0; Bustyp = 3; /Value 3 = Fieldbus Le_PA_DATA = 3; /Value 3 = Rrocess Input Words Le_PE_DATA = 3; /Value 3 = Rrocess Output Words/ End Initialize()/*= Interrupt Subroutine=*/Interrupt() _TouchProbe( TP_DIS1 ); M_State = 4 ; Position_2 = H507; Position_3 = H507 + 10000; / Vacuum On 10000 Position_4 = H507 + 345834; / 345834 Position_5 = H507 + 140000; / Vac Cups Ret 200000 Position_6 = H507 + 320000; / Slow Speed 400000 Position_6a = H507 + 363000; / Vac Cup Ext 411000 Position_7 = H507 + 421000; / Vac Rel vorher 250000 / 425000 Position_8 = H507 + 438000; / Blow-off off 438000 Position_9 = H507 + 850000; / Conv Rel 850000 Position_10 = H507 + 870000; / 870000 Ref_Pos = H507 + 415000; / 275000 Load_Pos_0 = H507 -10000; Load_Pos_1 = H507 + 80000; / Vac Cups Ret Load_Pos_2 = H507 + 200000; / Slow Speed Load_Pos_3 = H507 + 230000; / Vac Cups Ext Vac On Load_Pos_4 = H507 + 280000; / Load_Pos_5 = H507 + 250000; / Ret Vac Cup Load_Pos_6 = H507 + 248000; / Low Speed Load_Pos_6a = H507 + 220000; / Med Speed Load_Pos_7 = H507 + 185000; / Hi Speed Ext Vac Cup Load_Pos_8 = H507 + 70000; / Low Speed Load_Pos_9 = H507 + 40000; / Vac Rel Load_Pos_10 = H507 + 2000; / Conv Rel return; / End Interrupt()/* Cycling Subroutine. */Cycling() if (!E_DriveRel) M_State = 0; _BitClear (PE1_Output, A_ArmProbePos ); _BitClear (PE1_Output, A_ArmReferencePos); _AxisStop(AS_RSTOP); TargetPos = SetpointPos; / End Cycling/* Manual Mode Subroutine*/ManualMode()/ Stacker Arm Up and Down if(E_ArmRight | E_ArmLeft ) _AxisStop( AS_ENABLE ); V_MotorRight = V_MotorLeft = Man_Speed_0; _SetSys (SS_POSSPEED, V_MotorRight); if (E_ArmRight & (!E_ArmLeft) / Klemmen abfragen _GoAbs(GO_NOWAIT, (ActPos_Mot + 409600); if (E_ArmLeft & (!E_ArmRight) _GoAbs(GO_NOWAIT, (ActPos_Mot - 409600); if(!E_ArmRight & !E_ArmLeft ) _AxisStop( AS_RSTOP ); TargetPos = SetpointPos; / Vacuum and Release if (E_VacManMode) _BitSet(OptOutpIPOS, A_VacuumOnSol); _BitClear (OptOutpIPOS, A_VacuumOffSol); if (E_RelManMode & !E_VacManMode) _BitSet(OptOutpIPOS, A_VacuumOffSol); _BitSet(OptOutpIPOS, A_BlowOffSol); _BitClear (OptOutpIPOS, A_VacuumOnSol); else _BitClear (OptOutpIPOS, A_BlowOffSol); / Vacuum Cup In Out Sequence if (E_CupInOutManMode & E_CupExtendedProx & !(M_Merker & Bit_0) _BitSet(OptOutpIPOS, A_CupRetractSol); _BitClear (OptOutpIPOS, A_CupExtendSol); _BitSet (M_Merker, Bit_0); if (E_CupInOutManMode & !E_CupExtendedProx & !(M_Merker & Bit_0) _BitClear(OptOutpIPOS, A_CupRetractSol); _BitSet (OptOutpIPOS, A_CupExtendSol); _BitSet (M_Merker, Bit_0); if (!E_CupInOutManMode) _BitClear (M_Merker, Bit_0);/* if (E_CupExtendedProx) _BitClear (OptOutpIPOS, A_CupExtendSol); if (E_CupRetractedProx) _BitClear (OptOutpIPOS, A_CupRetractSol); */ / End ManualMode()/*= Automatic Mode Subroutine =*/AutomaticMode()/ Scope474 = StatusWord; / only for testing if (E_Start_Cycle & !E_Stop_Cycle & E_Touchp_1 & E_DriveEnable & E_DriveRel & (M_State = 0) & (M_Fault = 0 ) _BitClear (PE1_Output, A_ConvRel); _BitSet (OptOutpIPOS, A_CupExtendSol); _BitClear(OptOutpIPOS, A_CupRetractSol); M_State = 1 ; M_Stacking = 1 ; M_Loading = 0 ; if (E_LoadCycleStart & !E_Stop_Cycle & E_Touchp_1 & E_DriveEnable & E_DriveRel & (M_State = 0) & (M_Fault = 0 ) _BitClear (PE1_Output, A_ConvRel); M_State = 1 ; M_Stacking = 0 ; M_Loading = 1 ; if (E_Stop_Cycle) M_State = 0 ; M_Fault = 0 ; _AxisStop( AS_RSTOP ); _TouchProbe( TP_DIS1 ); _BitClear (PE1_Output, A_ArmReferencePos); _BitClear (PE1_Output, A_ArmProbePos); _BitClear (OptOutpIPOS, A_CupExtendSol); _BitClear (OptOutpIPOS, A_CupRetractSol); switch (M_State) case 1: _SetSys( SS_ACTPOS, Pos_0 ); _BitSet(H484,1); _Wait(2); ActPos_Mot = 0; TargetPos = 0; _Wait(2); _BitClear(H484,1); _TouchProbe( TP_EN1 ); V_MotorRight = V_MotorLeft = Auto_Speed_0; _SetSys (SS_POSSPEED, V_MotorRight); _GoAbs (GO_NOWAIT, Position_1); M_State = 2 ; break ; case 2: /Check Drive Stopped, Start Position not Found if (StatusWord & Bit_19) ( M_State = 3 ); break ; case 3: /Start Position Not Found _AxisStop( AS_RSTOP ); _BitSet(PE1_Output, A_StackerFault); _TouchProbe( TP_DIS1 ); M_State = 0 ; M_Fault = 1 ; break ; case 4: / Start Position Found if (!E_ReferenceRack) M_State = 5 ; else M_State = 8 ; break ; case 5: / Move to Rack Reference Position V_MotorRight = V_MotorLeft = Auto_Speed_1; _SetSys (SS_POSSPEED, V_MotorRight); _GoAbs (GO_NOWAIT, Ref_Pos); / Rack Reference Position M_State = 6 ; break ; case 6: / Release Rack Reference Motion (ge鋘dert: Stadelmeier if (StatusWord & Bit_19) & E_CupExtendedProx) / 15.09.03) M_State = 7 ; _BitSet(PE1_Output, A_ArmReferencePos); _BitSet(PE1_Output, A_ArmProbePos); break ; case 7: if (E_Continue_Cycle) if (!E_ReferenceRack & (M_Stacking = 1) _BitClear (OptOutpIPOS, A_VacuumOnSol); _BitSet (OptOutpIPOS, A_VacuumOffSol); _BitSet(OptOutpIPOS, A_BlowOffSol); _Wait(500); _BitClear (PE1_Output, A_ArmReferencePos); _