freetype ROTATION == { Clockwise , CounterClock , rIdle } freetype ARM_MOTOR == { Extend , Retract , aIdle }
freetype ON_OFF == { Off, On } typealias ANGLE == FLOAT typealias EXTENSION == FLOAT static function Arm1ToTable == 50.0 static function Arm2ToPress == 35.0 static function Arm2ToDepBelt == fsub(0.0,45.0) static function Arm1ToPress == fsub(0.0,90.0) static function Retracted == 0.0 static function OverTable == 0.5208 static function Arm2IntoPress == 0.7971 static function OverDepBelt == 0.5707 static function Arm1IntoPress == 0.6458 dynamic function RobotRotationMot : ROTATION initially rIdle dynamic function Arm1Mot : ARM_MOTOR initially aIdle dynamic function Arm2Mot : ARM_MOTOR initially aIdle dynamic function Arm1Mag : ON_OFF initially Off dynamic function Arm2Mag : ON_OFF initially Off output function Arm1Ext : EXTENSION output function Arm2Ext : EXTENSION output function Angle : ANGLE derived relation ArmsRectracted == Arm1Ext = Retracted and Arm2Ext = Retracted derived relation RobotIdle == RobotRotationMot = rIdle and Arm1Mot = aIdle and Arm2Mot = aIdle derived relation WaitingInUnloadTablePos == Angle = Arm1ToTable and ArmsRectracted and RobotIdle and Arm1Mag = Off and Arm2Mag = Off derived relation WaitingInUnloadPressPos == Angle = Arm2ToPress 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 WaitingInLoadPressPos == Angle = Arm1ToPress and ArmsRectracted and RobotIdle and Arm1Mag = On and Arm2Mag = Off derived relation ExtendingArm1ToTable == Angle = Arm1ToTable and Arm1Mot = Extend derived relation ExtendingArm2ToPress == Angle = Arm2ToPress and Arm2Mot = Extend derived relation ExtendingArm2ToDepBelt == Angle = Arm2ToDepBelt and Arm2Mot = Extend derived relation ExtendingArm1ToPress == Angle = Arm1ToPress and Arm1Mot = Extend derived relation ExtendedOverTable == Angle = Arm1ToTable and Arm1Ext = OverTable and Arm1Mot = aIdle derived relation ExtendedArm2IntoPress == Angle = Arm2ToPress and Arm2Ext = Arm2IntoPress and Arm2Mot = aIdle derived relation ExtendedOverDepBelt == Angle = Arm2ToDepBelt and Arm2Ext = OverDepBelt and Arm2Mot = aIdle derived relation ExtendedArm1IntoPress == Angle = Arm1ToPress and Arm1Ext = Arm1IntoPress and Arm1Mot = aIdle derived relation RetractingArm1ToTable == Angle = Arm1ToTable and Arm1Mot = Retract derived relation RetractingArm2ToPress == Angle = Arm2ToPress and Arm2Mot = Retract derived relation RetractingArm2ToDepBelt == Angle = Arm2ToDepBelt and Arm2Mot = Retract derived relation RetractingArm1ToPress == Angle = Arm1ToPress and Arm1Mot = Retract derived relation Arm1ToTableCompleted == RetractingArm1ToTable and Arm1Ext = Retracted derived relation Arm2ToPressCompleted == RetractingArm2ToPress and Arm2Ext = Retracted derived relation Arm2ToDepBeltCompleted == RetractingArm2ToDepBelt and Arm2Ext = Retracted derived relation Arm1ToPressCompleted == RetractingArm1ToPress and Arm1Ext = Retracted derived relation interval : FLOAT * FLOAT * FLOAT == fn(value,a,b) -> value >= a and value <= b 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 MovingToLoadDepBeltPos == ArmsRectracted and Arm1Mot = aIdle and Arm2Mot = aIdle and RobotRotationMot = CounterClock and Arm1Mag = On and Arm2Mag = On and interval(Angle, Arm2ToDepBelt, Arm2ToPress) 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 MovingToUnloadTablePos == ArmsRectracted and Arm1Mot = aIdle and Arm2Mot = aIdle and RobotRotationMot = Clockwise and Arm1Mag = Off and Arm2Mag = Off and interval(Angle,Arm1ToPress, Arm1ToTable) derived relation UnloadPressPosReached == Angle = Arm2ToPress derived relation LoadDepBeltPosReached == Angle = Arm2ToDepBelt derived relation LoadPressPosReached == Angle = Arm1ToPress derived relation UnloadTablePosReached == Angle = Arm1ToTable derived relation PressInUnloadPosition == PressInBottomPosition and PressMot = udIdle derived relation TableInUnloadPosition == ErtInTopPosition and MaxRotation derived relation TableReadyForUnloading == TableInUnloadPosition and TableLoaded derived relation PressReadyForUnloading == PressInUnloadPosition and PressLoaded derived relation PressReadyForLoading == PressInLoadPosition and not ( PressLoaded ) derived relation PressInLoadPosition == PressInMiddlePosition and PressMot = udIdle 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 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 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_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 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