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
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
|
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;
//-------------------DEFINE Motors------------------
//const Int32 LDANCER_Motor = 16;
//const Int32 LDRIVING_Motor = 18;
//const Int32 LLOADING_Motor = 19;
//const Int32 LDANCER_Motor = 16;
//const Int32 LDRIVING_Motor = 18;
//const Int32 LLOADING_Motor = 19;
const Int32 LDANCER1_Motor = 16;
const Int32 LDANCER2_Motor = 17;
const Int32 LDRIVING_Motor = 18;
const Int32 LLOADING_Motor = 19;
const Int32 RDANCER_Motor = 21;
//const Int32 RDRIVING_Motor = 22;
//const Int32 MAGNET_Driver = 23; //replace RLOADARM
const Int32 RLOADING_Motor = 24;
//-------------------DEFINE Motors Parameters------------------
const Int32 F1_MOTO_LLOADING_TX1 = 0x60000232;
const Int32 F1_MOTO_LLOADING_TX0 = 0x60000230;
const Int32 F1_MOTO_RLOADING_TX1 = 0x60000202;
const Int32 F1_MOTO_RLOADING_TX0 = 0x60000200;
//-------------------DEFINE Limit Switch---------------
const Int32 F1_LS_02_Direct =0x60000030;
const Int32 F1_LS_03_Direct =0x60000040;
const Int32 F1_LS_RLOADMOTOR_UP = 0x0040 ;
const Int32 F1_LS_RLOADMOTOR_DN = 0x0080 ;
const Int32 F1_LS_LLOADMOTOR_UP = 0x0100 ;
const Int32 F1_LS_LLOADMOTOR_DN = 0x0200 ;
const Int32 F1_LS_LDANCER1_UP = 0x1000;
const Int32 F1_LS_LDANCER1_DN = 0x2000;
const Int32 F1_LS_LDANCER2_UP = 0x0400;
const Int32 F1_LS_LDANCER2_DN = 0x0800;
const Int32 F1_LS_LLOADRAM_LF = 0x0080;
const Int32 F1_LS_LLOADRAM_RT = 0x0040;
const Int32 F1_LS_LSPARE2 = 0x0004; //up
//const Int32 F1_LS_LLOADMOTOR_UP = 0x0100 ;
const Int32 F1_LS_RDANCER_DN = 0x0200;
const Int32 F1_LS_RLOADRAM_DN = 0x0020;
const Int32 F1_LS_LSPARE1 = 0x0008; //dn
//include "..\..\Defines\Tango_Defines_Basic.cs"
//include "Tango_Defines_LimitSwitch.cs"
//include "Tango_Defines_Motors.cs"
//include "Tango_Defines_SSI.cs"
//include "C:\To_Backup\Mulberry\Mulberry_current\Customers\Twine\Tango\LAB_Tests\Main_PCB\ATP_Test\Scripts\Defines\Tango_Defines_Basic.cs"
//include "C:\To_Backup\Mulberry\Mulberry_current\Customers\Twine\Tango\LAB_Tests\Main_PCB\ATP_Test\Scripts\Defines\Tango_Defines_SSI.cs"
void OnExecute(StubManager stubManager)
{
for (int i=0;i<1;i++)
{
Move_Motor(LDANCER1_Motor,false,F1_LS_02_Direct,F1_LS_LDANCER1_UP,450);
Move_Motor(LDANCER2_Motor,false,F1_LS_02_Direct,F1_LS_LSPARE2,450);
Motor_SetParam(F1_MOTO_LLOADING_TX1,F1_MOTO_LLOADING_TX0,0x0a,0x0a000000); //set kval ran to 0x2c- 0.9A
Thread.Sleep(200);
Move_Motor(LLOADING_Motor,true,F1_LS_02_Direct,F1_LS_LLOADMOTOR_UP,300);
Motor_SetParam(F1_MOTO_LLOADING_TX1,F1_MOTO_LLOADING_TX0,0x0a,0x07000000); //set kval ran to 0x2c- 0.6A
Thread.Sleep(200);
Move_Motor(RDANCER_Motor,true,F1_LS_03_Direct,F1_LS_RLOADRAM_DN,10);
Motor_SetParam(F1_MOTO_RLOADING_TX1,F1_MOTO_RLOADING_TX0,0x0a,0x0a000000); //set kval ran to 0x14,0.65A
Thread.Sleep(200);
Move_Motor(RLOADING_Motor,false,F1_LS_03_Direct,F1_LS_RLOADMOTOR_UP,200);
Motor_SetParam(F1_MOTO_RLOADING_TX1,F1_MOTO_RLOADING_TX0,0x0a,0x07000000); //set kval ran to 0x14,0.65A
Thread.Sleep(200);
Thread.Sleep(2000);
Motor_SetParam(F1_MOTO_RLOADING_TX1,F1_MOTO_RLOADING_TX0,0x0a,0x0a000000); //set kval ran to 0x14,0.65A
Thread.Sleep(200);
Move_Motor(RLOADING_Motor,true,F1_LS_03_Direct,F1_LS_RLOADMOTOR_DN,200);
Motor_SetParam(F1_MOTO_RLOADING_TX1,F1_MOTO_RLOADING_TX0,0x0a,0x07000000); //set kval ran to 0x14,0.65A
Thread.Sleep(200);
Move_Motor(RDANCER_Motor,false,F1_LS_03_Direct,F1_LS_RDANCER_DN,10);
Motor_SetParam(F1_MOTO_LLOADING_TX1,F1_MOTO_LLOADING_TX0,0x0a,0x0a000000); //set kval ran to 0x2c- 0.9A
Thread.Sleep(200);
Move_Motor(LLOADING_Motor,false,F1_LS_02_Direct,F1_LS_LLOADMOTOR_DN,300);
Motor_SetParam(F1_MOTO_LLOADING_TX1,F1_MOTO_LLOADING_TX0,0x0a,0x0a000000); //set kval ran to 0x2c- 0.9A
Thread.Sleep(200);
Move_Motor(LDANCER2_Motor,true,F1_LS_02_Direct,F1_LS_LSPARE1,450);
Move_Motor(LDANCER1_Motor,true,F1_LS_02_Direct,F1_LS_LDANCER1_DN,450);
Thread.Sleep(2000);
}
}
void Move_Motor(UInt32 MotorID, bool Direction, UInt32 Register , UInt32 Bits, Double speed)
{
int counter ;
stubManager.Run<StubMotorRunResponse>("StubMotorRunRequest" ,MotorID,Direction,speed); //move Lloading up
counter =0;
while ( counter < 1500)
{
counter = counter + 1;
var response = stubManager.Run<StubFpgaReadRegResponse>("StubFpgaReadRegRequest" ,Register); //if LS_LLOADMOTOR_UP is up then move down
if ((response.Value & Bits) != Bits )
counter=1650;
Thread.Sleep(20);
}
stubManager.Run<StubMotorStopResponse>("StubMotorStopRequest" ,MotorID, 1); //hold motor
}
int Motor_SetParam(Int32 HighAdr, Int32 LowAdr, Int32 ParaAddr, uint ParaData1)
{
Int32 Temp = 0;
Temp = (ParaAddr << 8 ) + ( (Int32) ParaData1 >> 24);
stubManager.Run<StubFpgaWriteRegResponse>("StubFpgaWriteRegRequest" ,HighAdr, (Temp & 0xffff));
Temp = ((Int32)ParaData1 >> 8) & 0xffff;
stubManager.Run<StubFpgaWriteRegResponse>("StubFpgaWriteRegRequest" ,LowAdr, (Temp));
return 1;
}
|
