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
88
89
90
|
using System;
using System.Text;
using System.Linq;
using System.Drawing;
using System.Diagnostics;
using System.Windows.Forms;
using System.Threading;
using System.Threading.Tasks;
using System.Collections.Generic;
using Tango.PMR.Stubs;
using Tango.Stubs;
const Int32 FPGA1 = 0x60000000;
const Int32 FPGA2 = 0x60000400;
const Int32 FPGA3 = 0x60000800;
const Int32 F1_LS_03_Direct = 0x040 ; // Reads the direct values that are currently being sent to the fpga. rsv rsv rsv rsv F1_SW_SPOOL_EXISTS F1_SW_SPARE F1_LS_RDANCER_DOWN F1_LS_RDANCER_UP F1_LS_RLOADMOTOR_DOWN F1_LS_RLOADMOTOR_UP F1_LS_RLOADRAM_DOWN F1_LS_RLOADRAM_UP F1_LS_RSPARE1 F1_LS_RSPARE2 F1_LS_SCREW_LEFT F1_LS_SCREW_RIGHT
//include "..\..\Defines\Tango_Defines_Basic.cs"
//include "Tango_Defines_Motors.cs"
public void OnExecute(StubManager stubManager)
{
Int32 position=30;
UInt32 temp ;
Int32 Bit = 1;
stubManager.Run<StubMotorRunResponse>("StubMotorRunRequest" ,14, false, 250); //rotate screw Motor out.
Thread.Sleep(2000);
stubManager.Run<StubMotorStopResponse>("StubMotorStopRequest" ,14,3); //stop motor
Thread.Sleep(1000);
stubManager.Run<StubMotorRunResponse>("StubMotorRunRequest" ,14, true, 50);
stubManager.Write("\nMove screw in ");
Bit = 1;
int i=0;
while ((Bit != 0x0)&& (i<200) ) //wait until Limit Switch or timeout 200*50msec=10sec
{
Bit = GetBit(FPGA1, F1_LS_03_Direct,0);
Thread.Sleep(10);
i++;
}
stubManager.Run<StubMotorStopResponse>("StubMotorStopRequest" ,14,3); //stop motor
Thread.Sleep(1000);
stubManager.Run<StubMotorMovResponse>("StubMotorMovRequest" ,14, true, position );
Thread.Sleep(1000);
}
Int32 GetBit(Int32 Fpga, Int32 Adr, Int32 BitNo)
{
Int32 BitMask;
var RetVal = Fpga_Read_Reg(Fpga, Adr, 0);
BitMask = 0x1 << BitNo;
if ( ( (Int32) RetVal.Value & BitMask) == BitMask )
{
return 1;
}
else
{
return 0;
}
}
StubFpgaReadRegResponse Fpga_Read_Reg(Int32 Fpga, Int32 Addr, Int32 Verbose)
{
var response = stubManager.Run<StubFpgaReadRegResponse>("StubFpgaReadRegRequest" , Fpga + Addr);
response.Value = response.Value & 0xffff;
if (Verbose == 1) {
stubManager.Write("FPGA Reg. Read (FPGA Base, Addr, Data): (");
stubManager.WriteHex(Fpga,4);
stubManager.Write(", ");
stubManager.WriteHex(Addr,4);
stubManager.Write(", ");
stubManager.WriteHex(response.Value,4);
stubManager.Write(")");
}
return response;
}
|
