[r53]: / trunk / Main_Escape_PLC / Plc Logic / Application / Function Blocks / Puzzle / BalastTankPuzzleStateMachine / svnobj  Maximize  Restore  History

Download this file

88 lines (85 with data), 33.2 kB 

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
IF NOT xInit THEN	
	UM				(		sName := 'Normal Operation', 
							dwAllowsLeavingFromStates := PACK_ML.State.Stopped OR PACK_ML.State.Aborted OR PACK_ML.State.Idle, 
		IF SysStrCmp(Client.sCommandToPLC, Client.LockCommands[0]) =  0 THEN		
			//First Lock Function
			;
		ELSIF SysStrCmp(Client.sCommandToPLC, 'UnSuspend') =  0 THEN		
			UnSuspend();
		
	END_CASE
	Client.iCommandToPLC := 0;
	Client.sCommandToPLC := '';
	Client.bReadyToPLC := FALSE;
CASE UM.CurrentState OF
PACK_ML.State.Stopped:
	;
PACK_ML.State.Aborted:
PACK_ML.State.Idle:
PACK_ML.State.Complete :	
PACK_ML.State.Execute :
	// NOTE: Remember that most math has the Y axis as positive above the X.
    //final double result = Math.toDegrees(Math.atan2(deltaY, deltaX)); 
    //return (result < 0) ? (360d + result) : result;
	actualPositionLinear1 :=  AI_DistanceRight - 244;
	actualPositionLinear2 :=  AI_DistanceLeft - 244;
	deltaY := 250 - 0;
	deltaX := actualPositionLinear1 - actualPositionLinear2;
	angle := OSCAT_BASIC.ATAN2(deltaY,deltaX);
	targetPositionBoat := angle * 180 / 3.1415;		
	IF targetPositionBoat <= 0 THEN
		targetPositionBoat := 0;
	ELSIF targetPositionBoat >= 360 THEN
		targetPositionBoat := 360;
	END_IF
	BoatAngle :=  targetPositionBoat;
	IF Client.ActualStep >= 300 THEN
		AO_AnalogRefrence := AI_DistanceLeft;
	ELSE
		AO_AnalogRefrence := 0;	
	IF CMMT_ST.AxisError THEN
		Abort();
	IF BoatAngle >= 85 AND BoatAngle <= 95 THEN
		Steady := TRUE;
		Steady := FALSE;
		SendCommand(Command := CommandType.Media ,Value := 'Ballast');	
	IF Client.Mode = Mode.Mode1 THEN
		IF puzzleTimer.iProcess = 50 THEN
					CASE Client.Level OF
							speed := 100;						
							speed := 200;
							speed := 300;
							speed := 400;
							speed := 500;
					Client.ActualStep := 100;
					END_CASE		
				Mode.Mode3: // Analog					
						
		//10:
		//		Client.ActualStep := 11;
		//		Client.ActualStep := 20;				
		//11:
		//	Client.ActualStep := 10;			
		//	IF NOT CMMT_ST.Enabled THEN
		//		Client.ActualStep := 30;				
		//21:
		//	IF enable THEN
		//		Client.ActualStep := 10;
		//	IF NOT CMMT_ST.HomingValid THEN
		//		Client.ActualStep := 40;				
		//	home := CMMT_ST.Home(Position := 0,HomingMethod := 0);
		//		Client.ActualStep := Client.ActualStep := 32;	
		//	IF CMMT_ST.HomingValid THEN				
		//	END_IF		
		//40:
		100:
				Velocity:= speed, 
				Deceleration:= 500, 
				BufferMode:= MC_BUFFER_MODE.mcBuffered, 
				ErrorID=> , 
			IF bMove THEN
			END_IF
			IF CMMT_ST.TargetReached THEN
		102:
		103:
			END_IF		
			CASE OSCAT_BASIC.RDM2(last :=0, low := 1,high := 4) OF
					Client.ActualStep := 210;