external function ForgingCompleted : BOOL

external function PressInTopPosition : BOOL

external function PressInMiddlePosition : BOOL

external function PressInBottomPosition : BOOL

dynamic relation  PressLoaded
 initially false

external function GripperOverFeedBelt : BOOL

external function GripperOverDepositBelt : BOOL

static function OnFeedBelt ==
 0.6593

static function SafeDistanceFromFeedBelt ==
 0.0

static function OnDepositBelt ==
 0.9230

typealias GRIPPER == FLOAT

freetype CRANE == { ToDepositBelt,
                    ToFeedBelt,
                    cIdle}

derived relation interval : FLOAT*FLOAT*FLOAT
 == fn (value,a,b) -> value >= a and value <= b

static function Arm1IntoPress ==
 0.6458

static function OverDepBelt ==
 0.5707

static function Arm2IntoPress ==
 0.7971

static function OverTable ==
 0.5208

static function Retracted==
 0.0

static function Arm1ToPress==
 fsub(0.0,90.0)

static function Arm2ToDepBelt==
 fsub(0.0,45.0)

static function Arm2ToPress==
 35.0

static function Arm1ToTable==
 50.0

typealias EXTENSION == FLOAT

typealias ANGLE == FLOAT

freetype ON_OFF == { Off,
                     On}

freetype ARM_MOTOR == { Extend,
                        Retract,
                        aIdle}

freetype ROTATION == { Clockwise,
                       CounterClock,
                       rIdle}

external function PieceInDepositBeltLightBarrier : BOOL

dynamic relation  DepositBeltReadyForLoading
 initially true

dynamic relation  PieceAtDepositBeltEnd
 initially false

dynamic relation  Critical
 initially false

dynamic relation  TableLoaded
 initially false

external function ErtInTopPosition : BOOL

external function ErtInBottomPosition : BOOL

external function MaxRotation : BOOL

external function MinRotation : BOOL

freetype UP_DOWN == { Up,
                      Down,
                      udIdle}

external function PieceInFeedBeltLightBarrier : BOOL

dynamic relation  FeedBeltFree
 initially true

dynamic relation  Delivering
 initially false

external function add_blank : BOOL

dynamic function  PressMot : UP_DOWN
 initially udIdle

external function GripperVerticalPos : GRIPPER

dynamic function  CraneMagnet : ON_OFF
 initially Off

dynamic function  CraneVerticalMot : UP_DOWN
 initially udIdle

dynamic function  CraneHorizontalMot : CRANE
 initially cIdle

derived relation TableInUnloadPosition==
 ErtInTopPosition and MaxRotation

external function Angle : ANGLE

external function Arm2Ext : EXTENSION

external function Arm1Ext : EXTENSION

dynamic function  Arm2Mag : ON_OFF
 initially Off

dynamic function  Arm1Mag : ON_OFF
 initially Off

dynamic function  Arm2Mot : ARM_MOTOR
 initially aIdle

dynamic function  Arm1Mot : ARM_MOTOR
 initially aIdle

dynamic function  RobotRotationMot : ROTATION
 initially rIdle

dynamic function  DepBeltMot : ON_OFF
 initially On

dynamic function  TableRotationMot : ROTATION
 initially rIdle

dynamic function  TableElevationMot : UP_DOWN
 initially udIdle

dynamic function  FeedBeltMot : ON_OFF
 initially On

derived relation MovingToBottomPosition==
 PressMot = Down

derived relation ClosedForForging==
 PressInTopPosition and PressMot = udIdle

derived relation MovingToTopPosition==
 PressLoaded and PressMot = Up

derived relation OpenForLoading==
 PressInMiddlePosition and PressMot = udIdle

derived relation MovingToMiddlePosition==
 not(PressLoaded) and PressMot = Up

derived relation OpenForUnloading==
 PressInBottomPosition and PressMot = udIdle

transition MOVING_DB ==
 block
  if CraneHorizontalMot = ToDepositBelt and GripperOverDepositBelt
   then CraneHorizontalMot := cIdle
        CraneVerticalMot := Down
  endif
  
  if GripperOverDepositBelt and CraneVerticalMot = Down and GripperVerticalPos >= OnDepositBelt
   then CraneVerticalMot := udIdle
  endif
  
 endblock

transition LOADING_FB ==
 block
  if CraneVerticalMot = Down and GripperVerticalPos >= OnFeedBelt and GripperOverFeedBelt
   then CraneVerticalMot := udIdle
        CraneMagnet := Off
  endif
  
  if CraneVerticalMot = udIdle and GripperVerticalPos >= OnFeedBelt and GripperOverFeedBelt and CraneHorizontalMot = cIdle and CraneMagnet = Off
   then CraneHorizontalMot := ToDepositBelt
        FeedBeltFree := false
  endif
  
 endblock

transition MOVING_FB ==
 block
  if CraneHorizontalMot = ToFeedBelt and GripperOverFeedBelt
   then CraneHorizontalMot := cIdle
  endif
  
 endblock

transition UNLOADING_DB ==
 block
  if CraneVerticalMot = udIdle and GripperVerticalPos >= OnDepositBelt and CraneMagnet = On
   then CraneVerticalMot := Up
  endif
  
  if CraneVerticalMot = Up and GripperVerticalPos = SafeDistanceFromFeedBelt
   then CraneVerticalMot := udIdle
        CraneHorizontalMot := ToFeedBelt
        PieceAtDepositBeltEnd := false
  endif
  
 endblock

derived relation WaitingToLoadFeedBelt==
 GripperOverFeedBelt and GripperVerticalPos = SafeDistanceFromFeedBelt and CraneHorizontalMot = cIdle and CraneVerticalMot = udIdle and CraneMagnet = On

derived relation WaitingToUnloadDepositBelt==
 GripperOverDepositBelt and GripperVerticalPos >= OnDepositBelt and CraneHorizontalMot = cIdle and CraneVerticalMot = udIdle and CraneMagnet = Off

derived relation PressInLoadPosition==
 PressInMiddlePosition and PressMot = udIdle

derived relation TableReadyForUnloading==
 TableInUnloadPosition and TableLoaded

derived relation PressInUnloadPosition==
 PressInBottomPosition and PressMot = udIdle

derived relation UnloadTablePosReached==
 Angle = Arm1ToTable

derived relation LoadPressPosReached==
 Angle = Arm1ToPress

derived relation LoadDepBeltPosReached==
 Angle = Arm2ToDepBelt

derived relation UnloadPressPosReached==
 Angle = Arm2ToPress

derived relation RetractingArm1ToPress==
 Angle = Arm1ToPress and Arm1Mot = Retract

derived relation RetractingArm2ToDepBelt==
 Angle = Arm2ToDepBelt and Arm2Mot = Retract

derived relation RetractingArm2ToPress==
 Angle = Arm2ToPress and Arm2Mot = Retract

derived relation RetractingArm1ToTable==
 Angle = Arm1ToTable and Arm1Mot = Retract

derived relation ExtendedArm1IntoPress==
 Angle = Arm1ToPress and Arm1Ext = Arm1IntoPress and Arm1Mot = aIdle

derived relation ExtendedOverDepBelt==
 Angle = Arm2ToDepBelt and Arm2Ext = OverDepBelt and Arm2Mot = aIdle

derived relation ExtendedArm2IntoPress==
 Angle = Arm2ToPress and Arm2Ext = Arm2IntoPress and Arm2Mot = aIdle

derived relation ExtendedOverTable==
 Angle = Arm1ToTable and Arm1Ext = OverTable and Arm1Mot = aIdle

derived relation ExtendingArm1ToPress==
 Angle = Arm1ToPress and Arm1Mot = Extend

derived relation ExtendingArm2ToDepBelt==
 Angle = Arm2ToDepBelt and Arm2Mot = Extend

derived relation ExtendingArm2ToPress==
 Angle = Arm2ToPress and Arm2Mot = Extend

derived relation ExtendingArm1ToTable==
 Angle = Arm1ToTable and Arm1Mot = Extend

derived relation RobotIdle==
 RobotRotationMot = rIdle and Arm1Mot = aIdle and Arm2Mot = aIdle

derived relation ArmsRectracted==
 Arm1Ext = Retracted and Arm2Ext = Retracted

derived relation DB_Stopped==
 DepBeltMot = Off

derived relation DB_CriticalRun==
 DepBeltMot = On and Critical

derived relation DB_NormalRun==
 DepBeltMot = On and not(Critical)

transition MOVING_LOAD ==
 block
  if TableElevationMot = Down and ErtInBottomPosition
   then TableElevationMot := udIdle
  endif
  
  if TableRotationMot = CounterClock and MinRotation
   then TableRotationMot := rIdle
  endif
  
 endblock

transition MOVING_UNLOAD ==
 block
  if TableElevationMot = Up and ErtInTopPosition
   then TableElevationMot := udIdle
  endif
  
  if TableRotationMot = Clockwise and MaxRotation
   then TableRotationMot := rIdle
  endif
  
 endblock

derived relation StoppedInUnloadPosition==
 ErtInTopPosition and MaxRotation and TableElevationMot = udIdle and TableRotationMot = rIdle

derived relation StoppedInLoadPosition==
 ErtInBottomPosition and MinRotation and TableElevationMot = udIdle and TableRotationMot = rIdle

derived relation Stopped==
 FeedBeltMot = Off

derived relation CriticalRun==
 FeedBeltMot = On and Delivering

derived relation NormalRun==
 FeedBeltMot = On and not(Delivering)

derived relation UnloadingDepositBelt==
 CraneHorizontalMot = cIdle and GripperOverDepositBelt

transition MOVING_TO_LOWER ==
 block
  if MovingToBottomPosition and PressInBottomPosition
   then PressMot := udIdle
  endif
  
 endblock

transition CLOSED ==
 block
  if ClosedForForging and ForgingCompleted
   then PressMot := Down
  endif
  
 endblock

transition MOVING_TO_UPPER ==
 block
  if MovingToTopPosition and PressInTopPosition
   then PressMot := udIdle
  endif
  
 endblock

transition WAITING_LOAD_PRESS ==
 block
  if OpenForLoading and PressLoaded = true
   then PressMot := Up
  endif
  
 endblock

transition MOVING_TO_MIDDLE ==
 block
  if MovingToMiddlePosition and PressInMiddlePosition
   then PressMot := udIdle
  endif
  
 endblock

transition WAITING_UNLOAD_PRESS ==
 block
  if OpenForUnloading and PressLoaded = false
   then PressMot := Up
  endif
  
 endblock

transition WAITING_FB ==
 block
  if WaitingToLoadFeedBelt and FeedBeltFree
   then CraneVerticalMot := Down
  endif
  
 endblock

transition WAITING_DB ==
 block
  if WaitingToUnloadDepositBelt and PieceAtDepositBeltEnd
   then CraneMagnet := On
  endif
  
 endblock

transition ACTION_retraction ==
 block
  if RetractingArm1ToTable and Arm1Ext = Retracted
   then Arm1Mot := aIdle
        RobotRotationMot := CounterClock
        TableLoaded := false
  endif
  
  if RetractingArm2ToPress and Arm2Ext = Retracted
   then Arm2Mot := aIdle
        RobotRotationMot := CounterClock
        PressLoaded := false
  endif
  
  if RetractingArm2ToDepBelt and Arm2Ext = Retracted
   then Arm2Mot := aIdle
        RobotRotationMot := CounterClock
        DepositBeltReadyForLoading := false
  endif
  
  if RetractingArm1ToPress and Arm1Ext = Retracted
   then Arm1Mot := aIdle
        RobotRotationMot := Clockwise
        PressLoaded := true
  endif
  
 endblock

transition ACTION_proper ==
 block
  if ExtendedOverTable and Arm1Mag = On
   then Arm1Mot := Retract
  endif
  
  if ExtendedArm2IntoPress and Arm2Mag = On
   then Arm2Mot := Retract
  endif
  
  if ExtendedOverDepBelt and Arm2Mag = Off
   then Arm2Mot := Retract
  endif
  
  if ExtendedArm1IntoPress and Arm1Mag = Off
   then Arm1Mot := Retract
  endif
  
 endblock

transition ACTION_extension ==
 block
  if ExtendingArm1ToTable and Arm1Ext = OverTable
   then Arm1Mot := aIdle
        Arm1Mag := On
  endif
  
  if ExtendingArm2ToPress and Arm2Ext = Arm2IntoPress
   then Arm2Mot := aIdle
        Arm2Mag := On
  endif
  
  if ExtendingArm2ToDepBelt and Arm2Ext = OverDepBelt
   then Arm2Mot := aIdle
        Arm2Mag := Off
  endif
  
  if ExtendingArm1ToPress and Arm1Ext = Arm1IntoPress
   then Arm1Mot := aIdle
        Arm1Mag := Off
  endif
  
 endblock

derived relation PressReadyForLoading==
 PressInLoadPosition and not(PressLoaded)

derived relation PressReadyForUnloading==
 PressInUnloadPosition and PressLoaded

derived relation MovingToUnloadTablePos==
 ArmsRectracted and Arm1Mot = aIdle and Arm2Mot = aIdle and RobotRotationMot = Clockwise and Arm1Mag = Off and Arm2Mag = Off and interval(Angle,Arm1ToPress,Arm1ToTable)

derived relation MovingToLoadPressPos==
 ArmsRectracted and Arm1Mot = aIdle and Arm2Mot = aIdle and RobotRotationMot = CounterClock and Arm1Mag = On and Arm2Mag = Off and interval(Angle,Arm1ToPress,Arm2ToDepBelt)

derived relation MovingToLoadDepBeltPos==
 ArmsRectracted and Arm1Mot = aIdle and Arm2Mot = aIdle and RobotRotationMot = CounterClock and Arm1Mag = On and Arm2Mag = On and interval(Angle,Arm2ToDepBelt,Arm2ToPress)

derived relation MovingToUnloadPressPos==
 ArmsRectracted and Arm1Mot = aIdle and Arm2Mot = aIdle and RobotRotationMot = CounterClock and Arm1Mag = On and Arm2Mag = Off and interval(Angle,Arm2ToPress,Arm1ToTable)

derived relation Arm1ToPressCompleted==
 RetractingArm1ToPress and Arm1Ext = Retracted

derived relation Arm2ToDepBeltCompleted==
 RetractingArm2ToDepBelt and Arm2Ext = Retracted

derived relation Arm2ToPressCompleted==
 RetractingArm2ToPress and Arm2Ext = Retracted

derived relation Arm1ToTableCompleted==
 RetractingArm1ToTable and Arm1Ext = Retracted

derived relation WaitingInLoadPressPos==
 Angle = Arm1ToPress and ArmsRectracted and RobotIdle and Arm1Mag = On and Arm2Mag = Off

derived relation WaitingInLoadDepBeltPos==
 Angle = Arm2ToDepBelt and ArmsRectracted and RobotIdle and Arm1Mag = On and Arm2Mag = On

derived relation WaitingInUnloadPressPos==
 Angle = Arm2ToPress and ArmsRectracted and RobotIdle and Arm1Mag = On and Arm2Mag = Off

derived relation WaitingInUnloadTablePos==
 Angle = Arm1ToTable and ArmsRectracted and RobotIdle and Arm1Mag = Off and Arm2Mag = Off

transition DB_STOPPED ==
 block
  if DB_Stopped and not(PieceAtDepositBeltEnd)
   then DepBeltMot := On
        Critical := false
  endif
  
 endblock

transition DB_CRITICAL ==
 block
  if DB_CriticalRun and not(PieceInDepositBeltLightBarrier)
   then DepBeltMot := Off
        DepositBeltReadyForLoading := true
        PieceAtDepositBeltEnd := true
  endif
  
 endblock

transition DB_NORMAL ==
 block
  if DB_NormalRun and PieceInDepositBeltLightBarrier
   then Critical := true
  endif
  
 endblock

transition WAITING_UNLOAD ==
 block
  if StoppedInUnloadPosition and not(TableLoaded)
   then TableElevationMot := Down
        TableRotationMot := CounterClock
  endif
  
 endblock

transition WAITING_LOAD ==
 block
  if StoppedInLoadPosition and TableLoaded
   then TableElevationMot := Up
        TableRotationMot := Clockwise
  endif
  
 endblock

transition FB_CRITICAL ==
 block
  if CriticalRun and not(PieceInFeedBeltLightBarrier)
   then Delivering := false
        TableLoaded := true
  endif
  
 endblock

derived relation TableInLoadPosition==
 StoppedInLoadPosition

derived relation Crane_not_loaded==
 CraneHorizontalMot = ToDepositBelt or WaitingToUnloadDepositBelt or UnloadingDepositBelt

transition MOVING ==
 block
  if MovingToUnloadPressPos and UnloadPressPosReached
   then RobotRotationMot := rIdle
  endif
  
  if MovingToLoadDepBeltPos and LoadDepBeltPosReached
   then RobotRotationMot := rIdle
  endif
  
  if MovingToLoadPressPos and LoadPressPosReached
   then RobotRotationMot := rIdle
  endif
  
  if MovingToUnloadTablePos and UnloadTablePosReached
   then RobotRotationMot := rIdle
  endif
  
 endblock

transition WAITING ==
 block
  if WaitingInUnloadTablePos and TableReadyForUnloading
   then Arm1Mot := Extend
  endif
  
  if WaitingInUnloadPressPos and PressReadyForUnloading
   then Arm2Mot := Extend
  endif
  
  if WaitingInUnloadPressPos and not(PressLoaded)
   then RobotRotationMot := CounterClock
  endif
  
  if WaitingInLoadDepBeltPos and DepositBeltReadyForLoading
   then Arm2Mot := Extend
  endif
  
  if WaitingInLoadPressPos and PressReadyForLoading
   then Arm1Mot := Extend
  endif
  
 endblock

derived relation TableReadyForLoading ==
 TableInLoadPosition and not(TableLoaded)

transition FB_STOPPED ==
 block
  if Stopped and TableReadyForLoading
   then FeedBeltMot := On
        Delivering := true
  endif
  
 endblock

transition FB_NORMAL ==
 block
  if NormalRun and PieceInFeedBeltLightBarrier
   then FeedBeltFree := true
        if TableReadyForLoading
         then Delivering := true
         else FeedBeltMot := Off
        endif
        
  endif
  
 endblock

transition all_rules ==
 block
  if FeedBeltFree and Crane_not_loaded
   then if add_blank
         then FeedBeltFree := false
        endif
        
  endif
  
  WAITING_UNLOAD_PRESS
  MOVING_TO_MIDDLE
  WAITING_LOAD_PRESS
  MOVING_TO_UPPER
  CLOSED
  MOVING_TO_LOWER
  WAITING_DB
  UNLOADING_DB
  MOVING_FB
  WAITING_FB
  LOADING_FB
  MOVING_DB
  WAITING
  ACTION_extension
  ACTION_proper
  ACTION_retraction
  MOVING
  DB_NORMAL
  DB_CRITICAL
  DB_STOPPED
  WAITING_LOAD
  MOVING_UNLOAD
  WAITING_UNLOAD
  MOVING_LOAD
  FB_NORMAL
  FB_STOPPED
  FB_CRITICAL
 endblock

