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
|
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.UI;
const Int32 FPGA1 = 0x60000000;
const Int32 FPGA2 = 0x60000400;
const Int32 FPGA3 = 0x60000800;
const Int32 TEST_REG = 0x3f0; //F1 TEST_REG
//TX 1-MSW 0-LSW
const Int32 MOTO_LDRIVING_1 = 0x0222;
const Int32 MOTO_LDRIVING_0 = 0x0220;
const Int32 MOTO_LLOADING_1 = 0x0232;
const Int32 MOTO_LLOADING_0 = 0x0230;
const Int32 MOTO_LDANCER1_1 = 0x02c2;
const Int32 MOTO_LDANCER1_0 = 0x02c0;
const Int32 MOTO_LDANCER2_1 = 0x02d2;
const Int32 MOTO_LDANCER2_0 = 0x02d0;
const Int32 MOTO_LPIVOT1_1 = 0x302;
const Int32 MOTO_LPIVOT1_0 = 0x300;
int Motor_HighZ(Int32 Fpga, Int32 HighAdr, Int32 LowAdr)
{
//Send "highz" command - 1 byte
var response_tmp = stubManager.Run<StubFpgaWriteRegResponse>("StubFpgaWriteRegRequest" ,Fpga+TEST_REG, 0xa800);
var respons1 = stubManager.Run<StubFpgaWriteRegResponse>("StubFpgaWriteRegRequest" ,Fpga+HighAdr, 0xa800); // Stop x_HARD_HIZ 0xA8 + NOP
//Send "nop" command - last 2 bytes
response_tmp = stubManager.Run<StubFpgaWriteRegResponse>("StubFpgaWriteRegRequest" ,Fpga+TEST_REG, 0x0000);
respons1 = stubManager.Run<StubFpgaWriteRegResponse>("StubFpgaWriteRegRequest" ,Fpga+LowAdr, 0x0000);
return 1;
}
int Motor_Run(Int32 Fpga, Int32 HighAdr, Int32 LowAdr)
{
//Send "run" command - first 2 bytes
var response_tmp = stubManager.Run<StubFpgaWriteRegResponse>("StubFpgaWriteRegRequest" ,Fpga+TEST_REG, 0x5000);
var respons1 = stubManager.Run<StubFpgaWriteRegResponse>("StubFpgaWriteRegRequest" ,Fpga + HighAdr, 0x5000);//x_RUN 0x50 + Direction 0
//Send "run" command - last 2 bytes
response_tmp = stubManager.Run<StubFpgaWriteRegResponse>("StubFpgaWriteRegRequest" ,Fpga+TEST_REG, 0xffff);
respons1 = stubManager.Run<StubFpgaWriteRegResponse>("StubFpgaWriteRegRequest" ,Fpga + LowAdr, 0xffff);//Max Speed
return 1;
}
int Motor_Move(Int32 Fpga, Int32 HighAdr, Int32 LowAdr)
{
for (int i = 0; i < 8; i++)
{
//Send "Move" command - First 2 bytes - clockwise
var response_tmp = stubManager.Run<StubFpgaWriteRegResponse>("StubFpgaWriteRegRequest" ,Fpga + TEST_REG, 0x4000);
var respons1 = stubManager.Run<StubFpgaWriteRegResponse>("StubFpgaWriteRegRequest" ,Fpga + HighAdr, 0x4000); //x_MOVE 0x40 + Direction 0
//Send "Move" command - last 2 bytes
response_tmp = stubManager.Run<StubFpgaWriteRegResponse>("StubFpgaWriteRegRequest" ,Fpga + TEST_REG, 0x3200);
respons1 = stubManager.Run<StubFpgaWriteRegResponse>("StubFpgaWriteRegRequest" ,Fpga + LowAdr, 0x3200); // Position
Thread.Sleep(800);
//Send "Move" command - First 2 bytes - clockwise
response_tmp = stubManager.Run<StubFpgaWriteRegResponse>("StubFpgaWriteRegRequest" ,Fpga + TEST_REG, 0x4100);
respons1 = stubManager.Run<StubFpgaWriteRegResponse>("StubFpgaWriteRegRequest" ,Fpga + HighAdr, 0x4100);//x_MOVE 0x40 + Direction 1
//Send "Move" command - last 2 bytes
response_tmp = stubManager.Run<StubFpgaWriteRegResponse>("StubFpgaWriteRegRequest" ,Fpga + TEST_REG, 0x3200);
respons1 = stubManager.Run<StubFpgaWriteRegResponse>("StubFpgaWriteRegRequest" ,Fpga + LowAdr, 0x3200); //Position
Thread.Sleep(800);
}
return 1;
}
public void OnExecute(StubManager stubManager)
{
Motor_HighZ(FPGA1, MOTO_LDRIVING_1, MOTO_LDRIVING_0);
Motor_HighZ(FPGA1, MOTO_LLOADING_1, MOTO_LLOADING_0);
Motor_HighZ(FPGA1, MOTO_LDANCER1_1, MOTO_LDANCER1_0);
Motor_HighZ(FPGA1, MOTO_LDANCER2_1, MOTO_LDANCER2_0);
Motor_HighZ(FPGA1, MOTO_LPIVOT1_1, MOTO_LPIVOT1_0);
Motor_Run(FPGA1, MOTO_LDRIVING_1, MOTO_LDRIVING_0);
Thread.Sleep(6000);
Motor_HighZ(FPGA1, MOTO_LDRIVING_1, MOTO_LDRIVING_0);
Motor_Move(FPGA1, MOTO_LLOADING_1, MOTO_LLOADING_0);
Motor_HighZ(FPGA1, MOTO_LLOADING_1, MOTO_LLOADING_0);
Motor_Move(FPGA1, MOTO_LDANCER1_1, MOTO_LDANCER1_0);
Motor_HighZ(FPGA1, MOTO_LDANCER1_1, MOTO_LDANCER1_0);
Motor_Move(FPGA1, MOTO_LDANCER2_1, MOTO_LDANCER2_0);
Motor_HighZ(FPGA1, MOTO_LDANCER2_1, MOTO_LDANCER2_0);
Motor_Move(FPGA1, MOTO_LPIVOT1_1, MOTO_LPIVOT1_0);
Motor_HighZ(FPGA1, MOTO_LPIVOT1_1, MOTO_LPIVOT1_0);
}
|
