FUNCTION_BLOCK "DRVHandling"
TITLE =SERVOSTAR
//V2.0
//Datum    : 06.01.2004
//Name     : J.Schellhorn
//Homepage : www.DanaherMotion.net
AUTHOR : JSCHN
NAME : SSTR
VERSION : 2.0


VAR_INPUT
  BUS_Address : INT  := 3;	//Node-Address of Profibus / Profibus-Teilnehmer-Adresse
  NUDB : INT  := 100;	//No. of User-DataBlock / Nummer des Nutzdatenbausteins
  PERI_Address : INT  := 8;	//Periphery - address defined in hardware(PKW)/Peripherie adresse wie Hardware 
  SW_Enable : BOOL ;	//Drive enable (Software-Enable) / Freigabe Servo (Software)
  Reset_Fault : BOOL ;	//Reset Fault / Reset Fehler
  Jog_for : BOOL ;	//Jog + / Tippen +
  Jog_back : BOOL ;	//Jog - / Tippen -
  Ref_Start : BOOL ;	//Start reference move / Start Referenzfahrt
  Start_MT : BOOL ;	//Motion Task MT / Start Fahrsatz 
  Fast_Stop_Disable : BOOL ;	//Fast-Stop with disable drive / Not-Halt mit Einschaltsperre
  Fast_Stop_Enable : BOOL ;	//Fast-Stop without disabling drive / Not-Halt ohne Einschaltsperre
  Pause : BOOL ;	//Pause in Motion Task / Unterbrechung von Fahraufträgen
  Position_Reset : BOOL ;	//Reset position / Rücksetzen der Position
  Accept_Values : BOOL ;	//Accept new values / Neue Werte übernehmen
  Init_Drive : BOOL ;	//Initalize drive / Initialisierung des Reglers
END_VAR
VAR_OUTPUT
  Actual_Speed : WORD ;	//Actual speed / Istdrehzahl 
  Actual_Position : DWORD ;	//Actual position / Istposition
  Manufac_Stat : WORD ;	//Manufacturer Status / Herstellerstatus
  ZSW : WORD ;	//Status word of axis / Zustandsword der Achse
  Error : BOOL ;	//Error-bit / Sammelfehlerbit
  Pos_Error : BOOL ;	//Warning Pos.error / Warnung Schleppfehler angesprochen
  Moving : BOOL ;	//Motor is moving / Motor dreht
  Reference_ok : BOOL ;	//Reference ok / Referenz vorhanden
  In_Position : BOOL ;	//In position / Antrieb ist in Position
  ACK_ready_ok : BOOL ;	//Acknowledge operation ready without fault / Angeforderte Aktion fertig
  ACK_ready_fault : BOOL ;	//Acknowledge operation ready with fault Angeforderte Aktion fertig mit Fehler
  MT_active : BOOL ;	//Motion Task active / Fahrsatz aktiv
END_VAR
VAR
  nudbnr : WORD ;	
  NUDB_Adr : WORD ;	
  conv_stw : WORD ;	
  conv_stat : WORD ;	
  conv_zsw : WORD ;	
  conv_akku1 : DWORD ;	
  kfeh : BOOL ;	
  busy : BOOL ;	
  ofeh : BOOL ;	
  mfeh : BOOL ;	
  send : BOOL ;	
  redy : BOOL ;	
  status : WORD ;	
  parameter_nr : WORD ;	
  parameter_index : BYTE ;	
  read_write_flag : BOOL ;	
  start_flag : BOOL ;	
  aktiv : BOOL ;	
  fertig : BOOL ;	
  fehler : BOOL ;	
  ref_ok : BOOL ;	
  parametertyp : BYTE ;	
  parameter_high : WORD ;	
  parameter_low : WORD ;	
  reset_flag : BOOL ;	
  pf_ref_start : BOOL ;	
  ref_art_ok : BOOL ;	
  ba_ok : BOOL ;	
  enable_ok : BOOL ;	
  init_ok : BOOL ;	
  null_ok : BOOL ;	
  sypa : INT ;	
  modus : INT  := 2;	//Modus
  FlankeP : BOOL ;	// Bitmerker für positive Flanke
  FlankeN : BOOL ;	// Bitmerker für negative Flanke
  FlaMemP : BOOL ;	// Bitmerker für positive Flanke
  FlaMemN : BOOL ;	// Bitmerker für positive Flanke
  FlaP_RI : BOOL ;	// Bitmerker für positive Flanke
  FlaMemP_RI : BOOL ;	// Bitmerker für positive Flanke
  Init_Drive_Flag : BOOL ;	//Flag for Initalization Drive
  Accept_Values_Flag : BOOL ;	//Flag to accept new values
  Bit_Step : ARRAY  [1 .. 16 ] OF //Status Bits 
  BOOL ;	
  TaccOK : BOOL ;	//Beschleunigungszeitwert übernommen
  TdecOK : BOOL ;	//Verzögerungszeitwert übernommen
  PNU_OK : BOOL ;	//PNU und Parameter übernommen
  ZERO_OK : BOOL ;	//Nulltelegramm geschrieben
  NrKSTW : WORD ;	
  PKWReceive : ARRAY  [1 .. 8 ] OF BYTE ;	
  PZDReceive : ARRAY  [1 .. 12 ] OF BYTE ;	
  PKWSend : ARRAY  [1 .. 4 ] OF WORD ;	
  PZDSend : ARRAY  [1 .. 6 ] OF WORD ;	
  PKWPeriStart : WORD ;	
  PZDPeriStart : WORD ;	
  ErgPZD : INT ;	
  ErgPKW : INT ;	
  SendPKW : INT ;	
  Empf : INT ;	
  Stop_MT : BOOL ;	
  pf_flag_ref : BOOL ;	
  pf_flag_values : BOOL ;	
  Accept_flag : BOOL ;	
  te1 : WORD ;	
  te2 : WORD ;	
END_VAR
VAR_TEMP
  conv_nudb : WORD ;	
END_VAR
BEGIN
NETWORK
TITLE =Preset adresses User data block (NUDB)

      L     #NUDB; 
      T     #NUDB_Adr; 

      L     0; 
      T     #NrKSTW; 


      L     #PERI_Address; // startaddress PKW in periphery
      T     #PKWPeriStart; 

      L     #PERI_Address; 
      L     8; 
      +I    ; 
      T     #PZDPeriStart; // startaddress PZD in periphery

NETWORK
TITLE =Select function Reset Fault and Warnings

      L     #NUDB_Adr; 
      T     #conv_nudb; 
      AUF   DB [#conv_nudb]; // UserData DB

      U     #Reset_Fault; //Reset errors , warnings
      =     DBX   27.7; 
      U     #Reset_Fault; 
      U     DBX   47.7; // error / fault
      =     DBX   26.5; // Warning
NETWORK
TITLE =Set Message Outputs

      O     DBX   47.3; 
      =     #Error; 

      U     DBX   46.0; 
      =     #Pos_Error; 

      UN    DBX   54.4; // Motor is moving
      =     #Moving; 

      U     DBX   55.1; // Reference is done and ok
      U     DBX   55.3; 
      S     #Reference_ok; 
      UN    DBX   55.1; 
      R     #Reference_ok; 

      UN    DBX   55.0; // drive in position (depending of PEINPOS)
      U     DBX   55.3; 
      U     #Reference_ok; 
      =     #In_Position; 

      U     DBX   55.0; // Motion Task active
      =     #MT_active; 


NETWORK
TITLE =

      UN    #Init_Drive; 
      U     #Accept_Values; 
      =     #Accept_Values_Flag; 

      U     #Init_Drive; 
      =     #Init_Drive_Flag; 


NETWORK
TITLE =Initialisierung (Nulltelegramm)

      U     #Init_Drive_Flag; 
      UN    #FlaMemP_RI; 
      =     #FlaP_RI; 
      U     #FlaP_RI; 
      S     #FlaMemP_RI; 
      UN    #Init_Drive_Flag; 
      R     #FlaMemP_RI; 

      U     #ba_ok; 
      UN    #Init_Drive_Flag; 
      R     #Bit_Step[5]; 
      R     #Bit_Step[6]; 

      U     #Bit_Step[5]; 
      U     #fehler; 
      R     #Bit_Step[5]; 

      U     #Bit_Step[5]; 
      U     #fertig; 
      UN    #aktiv; 
      S     #Bit_Step[6]; 
      R     #Bit_Step[5]; 

      U     #Bit_Step[4]; 
      UN    #fertig; 
      U     #aktiv; 
      S     #Bit_Step[5]; 
      R     #Bit_Step[4]; 

      U     #Bit_Step[3]; 
      UN    #fertig; 
      UN    #aktiv; 
      S     #Bit_Step[4]; 
      R     #Bit_Step[3]; 

      U     #Bit_Step[2]; 
      UN    #aktiv; 
      U     #fertig; 
      S     #Bit_Step[3]; 
      R     #Bit_Step[2]; 

      U     #Bit_Step[1]; 
      U     #aktiv; 
      UN    #fertig; 
      S     #Bit_Step[2]; 
      R     #Bit_Step[1]; 

      U     #Init_Drive_Flag; //Reset bei Fehlern und Anlauf
      UN    #ba_ok; 
      UN    #Bit_Step[1]; 
      UN    #Bit_Step[2]; 
      UN    #Bit_Step[3]; 
      UN    #Bit_Step[4]; 
      UN    #Bit_Step[5]; 
      UN    #Bit_Step[6]; 
      S     #Bit_Step[1]; 

      U     #FlaP_RI; 
      R     #Bit_Step[1]; 
      R     #Bit_Step[2]; 
      R     #Bit_Step[3]; 
      R     #Bit_Step[4]; 
      R     #Bit_Step[5]; 
      R     #Bit_Step[6]; 
      R     #fertig; 
      R     #start_flag; 
      R     #read_write_flag; 
      R     #ba_ok; 
      R     DBX   26.2; 
      UN    #FlaP_RI; 
      SPB   M001; 
      L     0; 
      T     DBD   18; 
      T     DBD   22; 
      T     DBD   26; 
      T     DBD   30; 
      T     DBD   34; 

M001: NOP   0; 

      U     #fehler; 
      R     #start_flag; 
      R     #read_write_flag; 

      UN    #Bit_Step[1]; 
      UN    #Bit_Step[2]; 
      UN    #Bit_Step[3]; 
      SPB   M002; 
      L     3; 
      T     #parametertyp; 

M002: NOP   0; 

      U     #Bit_Step[1]; 
      S     #read_write_flag; 
      S     #start_flag; 

      U     #Bit_Step[3]; 
      R     #start_flag; 
      R     #read_write_flag; 

      UN    #Bit_Step[4]; 
      UN    #Bit_Step[5]; 
      UN    #Bit_Step[6]; 
      SPB   M003; 

      L     2; 
      T     #parametertyp; 

      L     W#16#3A2; 
      T     #parameter_nr; 

      L     DBW   80; 
      T     #parameter_low; 

      L     DBW   80; 

      L     0; 
      <I    ; 
      SPB   M054; 

      L     0; 
      SPA   M055; 
M054: NOP   0; 
      L     W#16#FFFF; 


M055: NOP   0; 
      T     #parameter_high; 
M003: NOP   0; 
      U     #Bit_Step[4]; 
      S     #read_write_flag; 
      S     #start_flag; 
      U     #Bit_Step[6]; 
      R     #start_flag; 
      R     #read_write_flag; 
      S     #ba_ok; 
NETWORK
TITLE =Setting Values for ACC and DCC and PNU
//Write Parameter with PNU declared in DBNU.DBW  84 and the belonging value in 
//DBNU.DBD  86. 
      U     #Accept_Values_Flag; 
      UN    #pf_flag_values; 
      =     #Accept_flag; 
      S     #pf_flag_values; 

      UN    #Accept_Values_Flag; 
      R     #pf_flag_values; 

      UN    #Accept_Values_Flag; 
      R     #PNU_OK; 
      R     #ZERO_OK; 

      UN    #Accept_flag; 
      SPB   M004; 

      U     #PNU_OK; 
      SPB   M004; 

      U     #ZERO_OK; 
      SPB   M005; 

      L     0; 
      T     DBW   18; 

      U(    ; 
      L     DBW   38; 
      L     0; 
      ==I   ; 
      )     ; 
      U     #Accept_Values_Flag; 
      UN    #PNU_OK; 
      S     #ZERO_OK; 

M005: NOP   0; 

      UN    #ZERO_OK; 
      SPB   M004; 

      L     2; 
      T     #parametertyp; 

      L     W#16#1; 
      T     #parameter_index; 

      L     DBW   84; //Betriebsartenumschaltung ??
      L     W#16#3A2; //PNU930
      <>I   ; 
      SPB   M006; 
      L     DBW   84; 
      T     #parameter_nr; 
      U     DBX   26.2; // PZD sperren (Bit 10 im STW)
      R     DBX   26.2; 
      SPA   M007; 

M006: NOP   0; 

      UN    DBX   83.2; 
      SPB   M008; 
      L     DBW   84; 
      T     #parameter_nr; 
      SPA   M009; 

M008: NOP   0; 

      L     DBW   84; 
      +     1600; 
      T     #parameter_nr; 

M009: NOP   0; 

      L     #parameter_nr; 
      L     2047; 
      <=I   ; 
      SPB   M007; 
      L     #parameter_nr; 
      +     -400; 
      T     #parameter_nr; 

      L     W#16#11; 
      T     #parameter_index; 

M007: NOP   0; 

      U     #Accept_flag; 
      U     DBX   83.1; 
      S     #read_write_flag; 

      U     #Accept_flag; 
      UN    DBX   83.1; 
      R     #read_write_flag; 

      UN    #read_write_flag; 
      SPB   M010; 

      L     DBW   86; 
      T     #parameter_high; 

      L     DBW   88; 
      T     #parameter_low; 

      SPA   M011; 

M010: NOP   0; 

      L     DBW   42; 
      T     DBW   76; 

      L     DBW   44; 
      T     DBW   78; 

M011: NOP   0; 
      U     #Accept_Values_Flag; 
      S     #start_flag; 

      U     #ZERO_OK; 
      U     #fertig; 
      U     #Accept_Values_Flag; 
      S     #PNU_OK; 
      R     #start_flag; 
      R     #ZERO_OK; 

      NOP   0; 


      UN    #PNU_OK; 
      SPB   M004; 
//
      L     3; 
      T     #parametertyp; 

      UN    #ZERO_OK; 
      S     #read_write_flag; 

      U     #fertig; 
      U     #Accept_Values_Flag; 
      U     #PNU_OK; 
      S     #ZERO_OK; 
      R     #start_flag; 
      S     DBX   26.2; 

      UN    #fertig; 
      U     #Accept_Values_Flag; 
      U     #PNU_OK; 
      S     #start_flag; 
      S     DBX   26.2; // PZD freigeben (Bit10 im STW)

M004: NOP   0; 

      U     #Bit_Step[6]; 
      U     #Init_Drive_Flag; 
      O     ; 
      U     #PNU_OK; 
      U     #ZERO_OK; 
      UN    #fehler; 
      =     #ACK_ready_ok; 
//      S     DBX   26.2

      U     #fehler; 
      U(    ; 
      O     #PNU_OK; 
      O     #ZERO_OK; 
      )     ; 
      O(    ; 
      U     DBX   38.4; 
      U     DBX   38.6; 
      U(    ; 
      O     #Accept_Values; 
      O     #Init_Drive; 
      )     ; 
      )     ; 
      =     #ACK_ready_fault; 


NETWORK
TITLE =Betriebsartenanwahl

      U     #SW_Enable; 
      U     #ba_ok; 
      =     DBX   27.0; 
      =     DBX   27.1; 

      U     DBX   47.0; 
      U     DBX   47.1; 
      U     #SW_Enable; 
      =     DBX   27.3; 

      U     #ba_ok; 
      U     DBX   27.0; // STW Bit0 : Einschalten
      U     DBX   27.3; // STW Bit3 : Betrieb freigeben
      U     DBX   47.2; 
      SPB   M012; 

      ON    #SW_Enable; 
      ON    #ba_ok; 
      SPB   M013; 

      U     #ba_ok; 
      U     DBX   27.0; // STW Bit0 : Einschalten
      U     DBX   27.3; // STW Bit3 : Betrieb freigeben
      SPB   M012; 

M013: NOP   0; 

      UN    #SW_Enable; 
      R     #enable_ok; 

      L     DBW   38; 
      SRW   12; 
      L     B#16#7; 
      ==I   ; 
      R     #init_ok; 

      SPA   M014; 

M012: NOP   0; 


NETWORK
TITLE =Antrieb enablen

      U     #enable_ok; 
      SPB   M015; 

      L     DBW   26; 
      L     W#16#43F; //Enablen des Servostar (Hardwareenable muss anstehen !!!)
      OW    ; 
      T     DBW   26; // STW beschreiben

      U     DBX   47.0; 
      U     DBX   47.1; 
      U     DBX   47.5; 
      UN    DBX   47.6; 
      S     #enable_ok; 

      ON    #SW_Enable; 
      O     DBX   47.6; 
      R     #enable_ok; 




      U     #enable_ok; 
      SPB   M015; 

      UN    #enable_ok; 
      R     DBX   27.3; 

      SPA   M014; 

M015: NOP   0; 

NETWORK
TITLE =Tippbetrieb

      UN    #Jog_for; //Tippen vorwärts angewählt
      UN    #Jog_back; 
      O     ; 
      U     #Jog_for; 
      U     #Jog_back; 
      SPB   M016; 

      U     #Jog_back; //Tippen rückwärts angewählt
      SPB   M017; 
      L     DBW   58; //Tippgeschwindigkeit (für vorwärts positiv)
      T     DBW   28; 
      SPB   M016; 
M017: NOP   0; 
      L     DBW   58; //Tippgeschwindigkeit
      L     -1; //(für rückwärts negativ) 
      *I    ; 
      T     DBW   28; 
M016: NOP   0; 
      U     #Jog_for; //Tippgeschwindigkeit auf Null und Steuerbit reset 
      UN    #Jog_back; 
      O     ; 
      UN    #Jog_for; 
      U     #Jog_back; 
      =     DBX   26.0; 
      R     #Stop_MT; 
NETWORK
TITLE =Referenzfahrt

      ON    #Ref_Start; 
      O     #ref_ok; 
      SPB   M014; 

      L     DBW   60; 
      T     DBW   28; 

      L     W#16#C3F; 
      T     DBW   26; 

      UN    DBX   55.0; 
      U     DBX   55.3; 
      U     DBX   55.1; 
      S     #ref_ok; 


M014: NOP   0; 
      UN    #Ref_Start; // Abbruch Referenzfahrt
      R     DBX   26.3; // reset Start Referenzfahrt

      UN    #Ref_Start; 
      UN    DBX   55.1; 
      R     #ref_ok; 

      U     #Fast_Stop_Disable; 
      =     DBX   27.2; 

      U     #Fast_Stop_Enable; 
      =     DBX   27.4; 

      UN    #Pause; 
      =     DBX   27.5; 


      U     #Position_Reset; 
      =     DBX   26.4; 

      L     DBW   26; 
      T     #conv_stw; 

      U     #Ref_Start; 
      FP    #pf_ref_start; 
      =     #pf_flag_values; 
      U     #pf_flag_values; 
      R     #ref_ok; 
      R     #Stop_MT; 
NETWORK
TITLE =
//alter FC106/FB106 / Fahrsatz aufrufen und abarbeiten
      L     2; 
      T     #modus; 
NETWORK
TITLE =Gesamtnetzwerk
// Fahrsatz starten
      U(    ; 
      L     #modus; 
      L     2; 
      <>I   ; // Betriebsart = Positionierung?
      )     ; 
      ON    DBX   55.1; 
      ON    #Reference_ok; 
      O     DBX   26.0; 
      O     #Ref_Start; 
      SPB   M018; // Nein, dann nicht bearbeiten

      UN    #Start_MT; // Bit zum Starten eines Fahrauftrages
      U     #FlaMemN; // negativer Flankenmerker
      =     #FlankeN; 
      R     #FlaMemN; 

      U     #Start_MT; 
      S     #FlaMemN; 


      U     #Start_MT; // Bit zum Starten eines Fahrsatzes
      UN    #FlaMemP; // Flankenmerker
      =     #FlankeP; 
      S     #FlaMemP; 

      UN    #Start_MT; 
      R     #FlaMemP; 

      U     #FlankeN; 
      U     DBX   83.0; // Vorwahl mit Toggeln
      SPB   M019; // Bei Flanke Fahrauftrag beenden

      U     #FlankeN; 
      UN    DBX   83.0; 
      SPB   M020; 

      U     #FlankeP; 
      SPB   M021; 

      SPA   M022; // zum Ausgang

M020: NOP   0; // Zeiger auf Steuerwort
      U     #FlankeN; 
      S     #Stop_MT; 

      SPA   M022; 

M019: NOP   0; 
      U     DBX   27.6; // Bit 6 im STW 
      R     DBX   27.6; 

      SPA   M022; 

M021: NOP   0; // 

      U     #Stop_MT; 
      R     #Stop_MT; 

      UN    DBX   83.0; 
      U     DBX   27.6; 
      R     DBX   27.6; 
      SPB   M022; 

      UN    DBX   27.6; // Abfrage Bit 6 im STW
      S     DBX   27.6; // Sollwertfreigabe / Start Fahrsatz

M022: NOP   0; 




      L     DBW   62; 
      L     0; 
      <>I   ; // Fahrsatznummer ungleich 0
      SPB   M023; // dann gespeicherten Fahrsatz starten
      NOP   0; // Programmteil für Direktfahraufträge
      UN    DBX   26.6; // Lade Steuerwort
      S     DBX   26.6; // Bit 14 = 1 (PZD - Direktfahrsatz)


      UN    DBX   27.3; // Bit 3 setzen und Betrieb freigeben
      S     DBX   27.3; 

      L     DBD   66; // Sollgeschwindigkeit
      T     DBD   28; // in Peripherie schreiben

      L     DBD   70; // Sollposition
      T     DBD   32; // in Peripherie schreiben

      L     DBW   64; // Fahrsatzart
      T     DBW   36; // in Peripherie schreiben
      SPA   M018; 

M023: NOP   0; // Programmteil für gespeicherte Fahraufträge, STW Bit14=0
      U     DBX   26.6; // Lade Steuerwort
      R     DBX   26.6; // Bit 14 = 0  (PZD (HSW) - Fahrsatznummer)

      UN    DBX   27.3; // Bit 3 setzen und Betrieb freigeben
      S     DBX   27.3; 


      L     DBW   62; 
      T     DBW   28; // im Prozeßdatenbereich ablegen

M018: NOP   0; 
      U     #Stop_MT; 
      R     DBX   27.3; 


NETWORK
TITLE =
//alter FC110/FB110 Transfer von Parametern, Ist- und Sollwerten zwischen 
//Steuerung und 
//Antrieb
      L     #NUDB_Adr; 
      T     #conv_nudb; 
      AUF   DB [#conv_nudb]; // Nutzdaten DB


      U     #start_flag; 
      SPB   M024; // liegt an

      L     DW#16#0; 
      T     DBW    0; 

      U     M      0.0; 
      ON    M      0.0; // immer 1
      R     #fertig; // alles zuruecksetzen
      R     #aktiv; 
      R     #fehler; 

      L     #status; 
      L     W#16#FF7F; 
      UW    ; // Status korrigieren (alt R M 239.7)
      T     #status; 

      SPA   M025; // -> Ende
M024: U     #fehler; 
      O     #fertig; // wenn fertig nicht nochmal lesen
      SPB   M026; 
      UN    #read_write_flag; // Parameter lesen
      SPB   M027; 
      SPA   M028; // Bei S7 direkter Sprung möglich, 16.5.1997
M027: NOP   0; // alt: U     M    239.7 // Laeuft Parameteraktion
      L     #status; 
      L     W#16#80; 
      UW    ; 
      L     128; 
      ==I   ; 
      SPB   M029; // Parameter lesen
      O     DBX    5.0; // PKW-Auftrag laeuft
      SPB   M025; // Zwischenstation zum Ende
      S     #aktiv; 

      L     #status; 
      L     W#16#180; // S M 239.7   //Parameteraktion starten
      OW    ; // S M 238.0   //Busy
      T     #status; 
      L     W#16#1000; // PKE/AK = 1 (lesen)
      L     #parameter_nr; // Parameternummer
      OW    ; // verknuepfen
      T     #conv_akku1; 
      L     #conv_akku1; 
      T     DBW   18; // PKE schreiben
// neu: 7.2.2000, Aufnahme des Index
      L     #parameter_index; 
      SLW   8; 
      T     #conv_akku1; 

      L     DBW   20; // IND lesen       
      L     W#16#FF; 
      UW    ; 
      L     #conv_akku1; 
      OW    ; 
      T     DBW   20; // IND schreiben, bis hier: 7.2.2000

//      L     W#16#1                      // PKW-Auftrag starten : Parameter lesen
//      T     #conv_akku1

//      L     #conv_akku1
//      T     DBW   20                    // abspeichern

M025: L     #status; //MW   238
      T     #status; 
      SPA   M026; 
      SPA   M030; // Zwischenstation zum Para-
      NOP   0; // meter schreiben
M029: NOP   0; // Ist Parameter schon da?
      NOP   0; // PKE im Empfangsfach, PPO-Typ2
      L     DBW   38; 
      L     W#16#7FF; 
      UW    ; // PNU extrahieren
      L     #parameter_nr; // =abgeschickter Parameternr.?
      <>I   ; 
      SPB   M025; // nein -> Status speichern, Rücksprung zum Ende
      NOP   0; // neu: 7.2.2000, Abfrage des Index
      NOP   0; // IND lesen vorbereiten
      L     DBW   40; 
      SRW   8; // 10.2.2000
      L     #parameter_index; // =abgeschicktem Index?
      <>I   ; 
      SPB   M025; // nein -> Status speichern, Rücksprung zum Ende
      L     DBW   38; // Überprüfung der Antwort, Wort/Doppelwort/generell OK
      L     W#16#F000; 
      UW    ; // AK ausblenden
      L     W#16#7000; // Parameterauftrag fehlerhaft ?
      ==I   ; 
      SPB   M031; // Ja !
      SPA   M032; // Nein !
M031: S     #fehler; // sonst Fehlerbit setzen
      L     W#16#FFFF; 
      T     #parameter_high; // High-Wort Parameter ungültig
      NOP   0; // PWE2 im Empfangsfach
      L     DBW   44; // holen
      T     #parameter_low; // Fehlernummer im Parameter-Low-Wort
      SPA   M025; // und beenden
M032: NOP   0; 
      L     DBW   38; // Überprüfung der Antwort, Wort/Doppelwort/generell OK
      SRW   12; // oberstes Nibble nach unten holen
      T     #parametertyp; // PTYP zuweisen
      L     2; // Doppelwortparameter ?
      ==I   ; 
      SPB   M033; // Ja !
      L     0; // Bei Wortparametern High-Wort immer auf 0 setzen
      T     #parameter_high; 
      SPA   M034; // Low-Wort aus DP-Fach holen
M033: NOP   0; 
      L     DBW   42; // PWE1 lesen
      T     #parameter_high; // ausgeben
M034: NOP   0; 
      L     DBW   44; // PWE2 lesen
      T     #parameter_low; // ausgeben
      SPA   M035; // Sprungmarke ueberspringen
M030: SPA   M028; 
M035: SPB   M036; // Fertig, VKE immer "1"
M036: S     #fertig; // Fertigbit setzen
      R     #aktiv; // Aktivbit zuruecknehmen
      R     #fehler; // Fehlerbit loeschen

      L     #status; 
      L     W#16#FE7F; 
      UW    ; // Status korrigieren (alt R M 239.7 / R M 238.0)
      T     #status; 

      SPA   M026; // Statuswort abspeichern
M028: NOP   0; 
NETWORK
TITLE =

      L     #status; //      U     M    239.7   // laeuft Parameteraktion?
      L     W#16#80; 
      UW    ; 
      L     128; 
      ==I   ; 
      SPB   M037; // ja, dann Antwort pruefen
      S     #aktiv; 

      L     #status; 
      L     W#16#180; 
      OW    ; // Status korrigieren (alt S M 239.7 / S M 238.0)
      T     #status; 


      L     #parametertyp; // Typ des Parameters
      L     1; // 1 = Wort, 2 = Doppelwort
      ==I   ; 
      SPB   M038; // nur ein Wort lesen, AK = 2
      L     #parametertyp; 
      L     2; 
      ==I   ; 
      SPB   M039; // Doppelwort lesen, AK = 3
      L     #parametertyp; // 7.5.99 bei PTYP = 3 Nulltelegramm erzeugen
      L     3; // 7.5.99
      ==I   ; // 7.5.99
      SPB   M040; // 7.5.99
      S     #fehler; // sonst mit VKE = 1 Fehler setzen
      R     #aktiv; // Aktivflag zurücksetzen

      L     #status; 
      L     W#16#FE7F; 
      UW    ; // Status korrigieren (alt R M 239.7 / R M 238.0)
      T     #status; 
      SPA   M026; 
M038: NOP   0; 
      L     W#16#2000; // PKE im Sendefach, AK = 2
      SPA   M041; 
M040: NOP   0; // 7.5.1999 PKE und PANR auf 0
      L     W#16#0; // 7.5.1999
      SPA   M042; // 7.5.1999
M039: NOP   0; 
      L     W#16#3000; // PKE im Sendefach, AK = 3
M041: L     #parameter_nr; 
      OW    ; // PKE/AK (schreiben)
      NOP   0; // mit Parameternummer
M042: T     #conv_akku1; 

      L     #conv_akku1; 
      T     DBW   18; // PKE ins DP-Fach eintragen
      NOP   0; // neu: 7.2.2000, IND auch verarbeiten
      L     #parametertyp; // Nulltelegramm beachten, 21.2.2000
      L     3; 
      <>I   ; 
      SPB   M043; // Normalfall
      L     0; 
      T     #conv_akku1; // 0 als Wert laden
      SPA   M044; 
M043: L     #parameter_index; 
      SLW   8; 
      T     #conv_akku1; 
      L     DBW   20; // IND lesen       
      L     W#16#FF; 
      UW    ; 
      L     #conv_akku1; 
      OW    ; 
M044: T     DBW   20; // IND schreiben, bis hier: 7.2.2000

      L     #parametertyp; // Doppelwort schreiben ?
      L     2; 
      ==I   ; 
      SPB   M045; 
      L     0; // sonst High-Wort auf 0 setzen, gilt auch für PTYP 3
      SPB   M046; 
M045: L     #parameter_high; // PWE1 im Sendefach
M046: T     #conv_akku1; 

      L     #conv_akku1; 
      T     DBW   22; // ins DP-Sendefach eintragen

      L     #parametertyp; // 7.5.1999 bei PTYP = 3 auch PALO mit 0 schreiben
      L     3; 
      <>I   ; 
      SPB   M047; 
      L     0; // 7.5.1999 0 laden für PTYP 3
      SPA   M048; 
M047: L     #parameter_low; // PWE2 im Sendefach
M048: T     #conv_akku1; 

      L     #conv_akku1; 
      T     DBW   24; // Low-Wert des Parameters
      L     2#1; // Start Servoantriebsdaten
      NOP   0; // = Kommunikationsteuerwort
      T     #conv_akku1; 

      L     #conv_akku1; 
      T     DBW    0; // PKW-Auftrag starten
      L     #status; 
      T     #status; 
      SPA   M026; 
M037: NOP   0; 
NETWORK
TITLE =

      L     #parametertyp; // 9.5.1999 PTYP = 3 (Nulltelegramm)?
      L     3; 
      <>I   ; 
      SPB   M049; // 9.5.1999 PTYP 1,2, dann weiter

      L     DBW   38; // 9.5.1999, PKE, PNU lesen
      L     0; 
      <>I   ; 
      SPB   M026; 
      U     DBX    5.4; // 9.5.1999, wenn Kommunikation gestört, Fehlerbit setzen
      SPB   M050; 
      SPA   M051; // 9.5.1999, Nulltelegramm erfolgreich geschrieben
M049: NOP   0; 
      L     DBW   38; // PKE lesen
      L     W#16#7FF; 
      UW    ; // PNU ausblenden
      L     #parameter_nr; // mit Parameternummer
      <>I   ; // vergleichen
      SPB   M026; // wenn ungleich, dann warten
      NOP   0; // neu: 7.2.2000, auch Index vergleichen
      NOP   0; // IND lesen vorbereiten
      L     DBW   40; 
      SRW   8; // 10.2.2000
      L     #parameter_index; // =abgeschicktem Index?
      <>I   ; 
      SPB   M026; // wenn ungleich, dann warten
      L     #parametertyp; // Antwort typabhängig auswerten
      L     1; 
      ==I   ; 
      SPB   M052; // Bei Wortschreiben mit Antwort-AK 1 vergleichen
      NOP   0; // sonst mit Antwort-AK 2
      L     DBW   38; // PKE lesen
      L     W#16#F000; 
      UW    ; // PKE lesen
      L     W#16#2000; 
      ==I   ; // AK ausblenden
      SPB   M051; // Parameterauftrag O.K.?
      SPA   M050; 
M052: NOP   0; 
      L     DBW   38; // PKE lesen
      L     W#16#F000; 
      UW    ; // AK ausblenden
      L     W#16#1000; 
      ==I   ; // Parameterauftrag O.K.?
      SPB   M051; 
M050: S     #fehler; // Fehlerbit setzen (VKE = 1)
      L     W#16#FFFF; 
      T     #parameter_high; // 16.6.97, High-Wort Parameter ungültig
      L     DBW   44; 
      T     #parameter_low; // 16.6.97, Fehlernummer im Parameter-Low-Wort
      SPA   M026; // und beenden
M051: S     #fertig; 
      R     #aktiv; // Fehlerbit setzen

      L     #status; 
      L     W#16#FE7F; 
      UW    ; // Status korrigieren 
      T     #status; 

M026: L     #status; 
      T     #status; 

      L     DBW   48; // aktuelle Drehzahl holen
      T     #Actual_Speed; 

      L     DBD   50; // aktuelle Position holen
      T     #Actual_Position; 

      L     DBW   54; // herstellerspezifischer Status
      T     #Manufac_Stat; 

      L     DBW   46; 
      T     #ZSW; // ZSW

      U     DBX   83.1; 
      SPB   M053; 

      L     DBW   42; 
      T     DBW   76; 

      L     DBW   44; 
      T     DBW   78; 

M053: NOP   0; 

NETWORK
TITLE =Receive consis. data from Profibus DP
// 
//
//
// PKW - Bereich, 8 Bytes
      CALL "DPRD_DAT" (// DPRD_DAT
           LADDR                    := #PKWPeriStart,// projektierte Startadresse in Peripherie
           RET_VAL                  := #ErgPKW,// Ergebnis des Lesens
           RECORD                   := #PKWReceive);

// PZD - Bereich, 12 Bytes
      CALL "DPRD_DAT" (// DPRD_DAT
           LADDR                    := #PZDPeriStart,// projektierte Startadresse in Peripherie, PZD
           RET_VAL                  := #ErgPZD,// Ergebnis des Lesens
           RECORD                   := #PZDReceive);

NETWORK
TITLE =Write received data (PKW) to datablock (NUDB)
//
// Daten wurden gelesen, nun koennen sie in den NUDB eingetragen werden
//
      L     #NrKSTW; 
      +     38; 
      T     #Empf; // Start der empfangenen PKW-Daten im Nutzdatenbaustein

      SLW   3; // Zeiger auf dieses Datenwort bilden
      LAR1  ; // Adreßregister 1 laden
      L     #PKWReceive[1]; 
      SLW   8; 
      L     #PKWReceive[2]; 
      OW    ; // PKW-Bereich, erstes Wort

      T     DBW [AR1,P#0.0]; // im NUDB speichern

      TAR1  ; 
      +     16; 
      LAR1  ; 
      L     #PKWReceive[3]; 
      SLW   8; 
      L     #PKWReceive[4]; 
      OW    ; // PKW-Bereich, zweites Wort

      T     DBW [AR1,P#0.0]; // im NUDB speichern

      TAR1  ; 
      +     16; 
      LAR1  ; 
      L     #PKWReceive[5]; 
      SLW   8; 
      L     #PKWReceive[6]; 
      OW    ; // PKW-Bereich, drittes Wort

      T     DBW [AR1,P#0.0]; // im NUDB speichern

      TAR1  ; 
      +     16; 
      LAR1  ; 
      L     #PKWReceive[7]; 
      SLW   8; 
      L     #PKWReceive[8]; 
      OW    ; // PKW-Bereich, viertes Wort

      T     DBW [AR1,P#0.0]; // im NUDB speichern, PKW komplett
NETWORK
TITLE =Write received data (PZD) to datablock (NUDB)
//
// PKW-Daten eingetragen, nun PZD-Daten
//
      L     #Empf; 
      +     8; 
      T     #Empf; // Start der empfangenen PZD-Daten im Nutzdatenbaustein

      SLW   3; // Zeiger auf dieses Datenwort bilden
      LAR1  ; // Adreßregister 1 laden
      L     #PZDReceive[1]; 
      SLW   8; 
      L     #PZDReceive[2]; 
      OW    ; // PZD-Bereich, erstes Wort

      T     DBW [AR1,P#0.0]; // im NUDB speichern

      TAR1  ; 
      +     16; 
      LAR1  ; 
      L     #PZDReceive[3]; 
      SLW   8; 
      L     #PZDReceive[4]; 
      OW    ; // PZD-Bereich, zweites Wort

      T     DBW [AR1,P#0.0]; // im NUDB speichern

      TAR1  ; 
      +     16; 
      LAR1  ; 
      L     #PZDReceive[5]; 
      SLW   8; 
      L     #PZDReceive[6]; 
      OW    ; // PZD-Bereich, drittes Wort

      T     DBW [AR1,P#0.0]; // im NUDB speichern

      TAR1  ; 
      +     16; 
      LAR1  ; 
      L     #PZDReceive[7]; 
      SLW   8; 
      L     #PZDReceive[8]; 
      OW    ; // PZD-Bereich, viertes Wort

      T     DBW [AR1,P#0.0]; // im NUDB speichern

      TAR1  ; 
      +     16; 
      LAR1  ; 
      L     #PZDReceive[9]; 
      SLW   8; 
      L     #PZDReceive[10]; 
      OW    ; // PZD-Bereich, fuenftes Wort

      T     DBW [AR1,P#0.0]; // im NUDB speichern

      TAR1  ; 
      +     16; 
      LAR1  ; 
      L     #PZDReceive[11]; 
      SLW   8; 
      L     #PZDReceive[12]; 
      OW    ; // PZD-Bereich, sechstes Wort

      T     DBW [AR1,P#0.0]; // im NUDB speichern, PZD komplett
NETWORK
TITLE =Send consist. data (PKW) from datablock (NUDB) to Profibus DP
//
// PKW-Daten aus dem NUDB holen und in Felder eintragen
//
      L     #NrKSTW; 
      +     18; 
      T     #SendPKW; // Startadresse des PKW-Sendebereichs im Nutzdatenbaustein

      SLW   3; // Zeiger auf dieses Datenwort bilden
      LAR1  ; // Adreßregister 1 laden

      L     DBW [AR1,P#0.0]; // aus NUDB holen
      T     #PKWSend[1]; // PKW erstes Wort geladen

      TAR1  ; 
      +     16; 
      LAR1  ; 

      L     DBW [AR1,P#0.0]; // aus NUDB holen
      T     #PKWSend[2]; // PKW zweites Wort geladen

      TAR1  ; 
      +     16; 
      LAR1  ; 

      L     DBW [AR1,P#0.0]; // aus NUDB holen
      T     #PKWSend[3]; // PKW drittes Wort geladen

      TAR1  ; 
      +     16; 
      LAR1  ; 

      L     DBW [AR1,P#0.0]; // aus NUDB holen
      T     #PKWSend[4]; // PKW viertes Wort geladen

      CALL "DPWR_DAT" (// DPWR_DAT
           LADDR                    := #PKWPeriStart,
           RECORD                   := #PKWSend,
           RET_VAL                  := #ErgPKW);// PKW gesendet

NETWORK
TITLE =Send consist. data (PZD) from datablock (NUDB) to Profibus DP
//
// PZD-Daten aus dem NUDB holen und in Felder eintragen
//
      L     #SendPKW; 
      +     8; 
      T     #SendPKW; // Startadresse des PZD - Sendebereichs im Nutzdatenbaustein

      SLW   3; // Zeiger auf dieses Datenwort bilden
      LAR1  ; // Adreßregister 1 laden

      L     DBW [AR1,P#0.0]; // aus NUDB holen
      T     #PZDSend[1]; // PZD erstes Wort geladen

      TAR1  ; 
      +     16; 
      LAR1  ; 

      L     DBW [AR1,P#0.0]; // aus NUDB holen
      T     #PZDSend[2]; // PZD zweites Wort geladen

      TAR1  ; 
      +     16; 
      LAR1  ; 

      L     DBW [AR1,P#0.0]; // aus NUDB holen
      T     #PZDSend[3]; // PZD drittes Wort geladen

      TAR1  ; 
      +     16; 
      LAR1  ; 

      L     DBW [AR1,P#0.0]; // aus NUDB holen
      T     #PZDSend[4]; // PZD viertes Wort geladen

      TAR1  ; 
      +     16; 
      LAR1  ; 

      L     DBW [AR1,P#0.0]; // aus NUDB holen
      T     #PZDSend[5]; // PZD fuenftes Wort geladen

      TAR1  ; 
      +     16; 
      LAR1  ; 

      L     DBW [AR1,P#0.0]; // aus NUDB holen
      T     #PZDSend[6]; // PZD sechstes Wort geladen

      CALL "DPWR_DAT" (// DPWR_DAT
           LADDR                    := #PZDPeriStart,
           RECORD                   := #PZDSend,
           RET_VAL                  := #ErgPZD);// PZD gesendet

END_FUNCTION_BLOCK