大家好！

在起重机自动化定位的过程中为了提高效率，尽量靠近目标位置才会减速，为了更加准确的定位，需要计算当前速度的下一刻时间的起重机的停止位置！

如下FB可以实现这个功能！

FUNCTION_BLOCK "ESP_Calc"
TITLE =Estimated Stop Position
//********************************************************************************
//
//FILE        : ESP (Estimated Stoping Position) calculations
//
//DESCRIPTION : 估算停止的位置
//
//Remarks     :
//
//Revision
//history     : KHy V1.0, first release,
//              cheppu V1.1, calculation fixed
//              30.6.05 cheppu V1.2, Hoist driving added
//
//Revised     : APK/MLI V2.0 Added new input parameter own error multiplier for 
//              hoist
//********************************************************************************
AUTHOR : 'APK/MLI'zhangwei.liao
VERSION : 2.0

VAR_INPUT
  CraneSPD : INT ; //Crane Actual speed (from the drive) +/- 10000
  Nom_SPD : DINT ; //Crane nominal max. speed, 1000 = 1 m/s
  StopDist : INT ; //Stop Distance mm
  MeasDelay : DINT ; //Delay in measurement signal 0 .. 1000 ms
  MesPosit : DINT ; //Measured crane position
  MeasUnits : DINT  := L#1; //measurement unit in millimeters (if in cm use L#10, if mm use L#1)
  A_SpeedRef : INT ; //Speed reference of the hoist
  A_ErrorMultiplier : DINT ; //Increase ESP according hoist speed
  MaxSpeed : DINT ; //Maximum value of PD1
END_VAR
VAR_OUTPUT
  ESP : DINT ; //Estimated Stop Position [mm]
END_VAR
VAR_TEMP
  Speed : DINT ; 
  ERROR : DINT ; 
  S_Dist_D : DINT ; 
  TMP_ESP : DINT ; 
  TMP_D : DINT ; 
END_VAR
BEGIN
NETWORK
TITLE =Convert the given speed reference to mm/s
//Millimeters per second..
      A(    ;
      A(    ;
      L     #CraneSPD;
      ITD   ;
      T     #Speed;
      SET   ;
      SAVE  ;
      CLR   ;
      A     BR;
      )     ;
      JNB   _001;
      L     #Speed;
      L     #Nom_SPD;
      *D    ;
      T     #Speed;
      AN    OV;
      SAVE  ;
      CLR   ;
_001: A     BR;
      )     ;
      JNB   _002;
      L     #Speed;
      L     #MaxSpeed;
      /D    ;
      T     #Speed;
_002: NOP   0;
NETWORK
TITLE =Calculate the error caused by measurement delay
//Crane nominal speed in mm * measurement delay = distance travelled during the
//delay time
      A(    ;
      L     #Speed;
      L     #MeasDelay;
      *D    ;
      T     #TMP_D;
      AN    OV;
      SAVE  ;
      CLR   ;
      A     BR;
      )     ;
      JNB   _003;
      L     #TMP_D;
      L     L#1000;
      /D    ;
      T     #ERROR;
_003: NOP   0;
NETWORK
TITLE =Increase "error" value if going down
//If hoist is going down, then multiply error value by 1.3
      A(    ;
      A(    ;
      L     #A_SpeedRef;
      L     0;
      <I    ;
      )     ;
      JNB   _004;
      L     #ERROR;
      L     #A_ErrorMultiplier;
      *D    ;
      T     #ERROR;
      AN    OV;
      SAVE  ;
      CLR   ;
_004: A     BR;
      )     ;
      JNB   _005;
      L     #ERROR;
      L     L#10;
      /D    ;
      T     #ERROR;
_005: NOP   0;
NETWORK
TITLE =Scale the ERROR into the used measurement value.
//If the input value is ZERO skip the line
      L     #MeasUnits;
      L     L#0;
      <>D   ;
      =     L     20.0;
      A     L     20.0;
      JNB   _006;
      L     #ERROR;
      L     #MeasUnits;
      /D    ;
      T     #ERROR;
_006: NOP   0;
      A     L     20.0;
      NOT   ;
      JNB   _007;
      L     0;
      T     #ERROR;
_007: NOP   0;
NETWORK
TITLE =Scale the stopping distance into the used measurement value.
//If the input value is ZERO skip the line.
      A(    ;
      L     #StopDist;
      ITD   ;
      T     #S_Dist_D;
      SET   ;
      SAVE  ;
      CLR   ;
      A     BR;
      )     ;
      A(    ;
      L     #MeasUnits;
      L     L#0;
      <>D   ;
      )     ;
      JNB   _008;
      L     #S_Dist_D;
      L     #MeasUnits;
      /D    ;
      T     #S_Dist_D;
_008: NOP   0;
NETWORK
TITLE =Calculation of ESP (Estimated Stop Position)
//1. Take the measured position as starting point
//2. Add error caused by measurement delays (Scaled according to nominal speed
//   and position measurement unit).
//3. Add stopping distance (scaled according to the nominal speed & Pos
//   measurement unit)
      A(    ;
      A(    ;
      A(    ;
      L     #MesPosit;
      T     #TMP_ESP;
      SET   ;
      SAVE  ;
      CLR   ;
      A     BR;
      )     ;
      JNB   _009;
      L     #TMP_ESP;
      L     #S_Dist_D;
      +D    ;
      T     #TMP_ESP;
      AN    OV;
      SAVE  ;
      CLR   ;
_009: A     BR;
      )     ;
      JNB   _00a;
      L     #TMP_ESP;
      L     #ERROR;
      +D    ;
      T     #TMP_ESP;
      AN    OV;
      SAVE  ;
      CLR   ;
_00a: A     BR;
      )     ;
      JNB   _00b;
      L     #TMP_ESP;
      T     #ESP;
_00b: NOP   0;
NETWORK
TITLE =End block with RLO = '1' and ENO = '1'

      ON    BR;
      O     BR;
      SAVE  ;
      BEC   ;
END_FUNCTION_BLOCK