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
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
|
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;
const Int32 MOTO_SCREW_TX0 = 0x0280;
const Int32 MOTO_SCREW_TX1 = 0x0282;
const Int32 MOTO_SCREW_RX0 = 0x0284;
const Int32 MOTO_SCREW_RX1 = 0x0286;
const Int32 MOTO_WINDER_TX0 = 0x290;
const Int32 MOTO_WINDER_TX1 = 0x292;
const Int32 MOTO_WINDER_RX0 = 0x294;
const Int32 MOTO_WINDER_RX1 = 0x296;
const Int32 MOTO_LDRIVING_TX1 = 0x0222;
const Int32 MOTO_LDRIVING_TX0 = 0x0220;
const Int32 MOTO_LDRIVING_RX1 = 0x0226;
const Int32 MOTO_LDRIVING_RX0 = 0x0224;
const Int32 MOTO_LLOADING_TX1 = 0x0232;
const Int32 MOTO_LLOADING_TX0 = 0x0230;
const Int32 MOTO_LDANCER1_TX1 = 0x02c2;
const Int32 MOTO_LDANCER1_TX0 = 0x02c0;
const Int32 MOTO_LDANCER2_TX1 = 0x02d2;
const Int32 MOTO_LDANCER2_TX0 = 0x02d0;
const Int32 MOTO_LDANCER2_A1_AMT_OF_WORDS = 0x02d8;
const Int32 MOTO_LPIVOT1_TX1 = 0x0302;
const Int32 MOTO_LPIVOT1_TX0 = 0x0300;
const Int32 MOTO_LPIVOT1_A1_AMT_OF_WORDS = 0x0308;
const Int32 MOTORDRIVER_HIGHZ_COMMAND_MSB = 0xa800;
const Int32 MOTORDRIVER_HIGHZ_COMMAND_LSB = 0x0000;
const Int32 MOTORDRIVER_RUN_COMMAND_MSB = 0x5000;
const Int32 MOTORDRIVER_RUN_COMMAND_LSB = 0x0000;
const Int32 MOTORDRIVER_MOVE_COMMAND_MSB = 0x4000;
const Int32 MOTORDRIVER_MOVE_COMMAND_LSB = 0x0000;
const Int32 MOTORDRIVER_RDCFG_COMMAND_MSB = 0x3800;
const Int32 MOTORDRIVER_RDCFG_COMMAND_LSB = 0x0000;
StubFpgaReadRegResponse Fpga_Read_Reg(Int32 Fpga, Int32 Addr)
{
var response = stubManager.Run<StubFpgaReadRegResponse>("StubFpgaReadRegRequest" , Fpga + Addr);
response.Value = response.Value & 0xffff;
stubManager.Write("\n\nFPGA 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(")");
response.Value = 0;
return response;
}
int Fpga_Write_Reg(Int32 Fpga, Int32 Addr, Int32 Data)
{
var respons1 = stubManager.Run<StubFpgaWriteRegResponse>("StubFpgaWriteRegRequest" ,Fpga + Addr, Data);
return 1;
}
int Motor_HighZ(Int32 Fpga, Int32 HighAdr, Int32 LowAdr)
{
Fpga_Write_Reg(FPGA1, HighAdr, MOTORDRIVER_HIGHZ_COMMAND_MSB);
Fpga_Write_Reg(FPGA1, LowAdr, MOTORDRIVER_HIGHZ_COMMAND_LSB);
return 1;
}
int Motor_Run(Int32 Fpga, Int32 HighAdr, Int32 LowAdr, Int32 Speed, Int32 Direction)
{
var RunCmdHigh = ( ((MOTORDRIVER_RUN_COMMAND_MSB >> 8) + (Direction & 0x1)) << 8 ) + ( (Speed & 0x0fffff) >> 16);
var RunCmdLow = (Speed & 0xffff);
Fpga_Write_Reg(FPGA1, HighAdr, RunCmdHigh);
Fpga_Write_Reg(FPGA1, LowAdr, RunCmdLow);
return 1;
}
int Motor_Move(Int32 Fpga, Int32 HighAdr, Int32 LowAdr, Int32 NumOfSteps, Int32 Direction)
{
var MoveCmdHigh = ( ((MOTORDRIVER_MOVE_COMMAND_MSB >> 8) + (Direction & 0x1)) << 8 ) + ( (NumOfSteps & 0x3fffff) >> 16);
var MoveCmdLow = (NumOfSteps & 0xffff);
Fpga_Write_Reg(FPGA1, HighAdr, MoveCmdHigh);
Fpga_Write_Reg(FPGA1, LowAdr, MoveCmdLow);
return 1;
}
int Motor_Move_Loop(Int32 Fpga, Int32 HighAdr, Int32 LowAdr, Int32 NumOfSteps, Int32 Direction, Int32 NumOfLoops)
{
for (int i = 0; i < NumOfLoops; i++)
{
Motor_Move(Fpga, HighAdr, LowAdr, NumOfSteps, Direction);
Thread.Sleep(1000);
Motor_Move(Fpga, HighAdr, LowAdr, NumOfSteps, Direction^0x1);
Thread.Sleep(1000);
}
return 1;
}
int Motor_ReadCfg(Int32 Fpga, Int32 HighAdr, Int32 LowAdr)
{
Fpga_Write_Reg(Fpga, HighAdr, MOTORDRIVER_RDCFG_COMMAND_MSB);
Fpga_Write_Reg(Fpga, LowAdr, MOTORDRIVER_RDCFG_COMMAND_LSB);
Fpga_Read_Reg(Fpga, HighAdr+4);
Fpga_Read_Reg(Fpga, LowAdr+4);
return 1;
}
public void OnExecute(StubManager stubManager)
{
Motor_HighZ(FPGA1, MOTO_WINDER_TX1, MOTO_WINDER_TX0);
//Motor_HighZ(FPGA1, MOTO_LLOADING_TX1, MOTO_LLOADING_TX0);
//Motor_HighZ(FPGA1, MOTO_LDANCER1_TX1, MOTO_LDANCER1_TX0);
//Motor_HighZ(FPGA1, MOTO_LDANCER2_TX1, MOTO_LDANCER2_TX0);
//Motor_HighZ(FPGA1, MOTO_LPIVOT1_TX1, MOTO_LPIVOT1_TX0);
Motor_ReadCfg(FPGA1, MOTO_WINDER_TX1, MOTO_WINDER_TX0);
//Motor_ReadCfg(FPGA1, MOTO_LLOADING_TX1, MOTO_LLOADING_TX0);
//Motor_ReadCfg(FPGA1, MOTO_LDANCER1_TX1, MOTO_LDANCER1_TX0);
//Motor_ReadCfg(FPGA1, MOTO_LDANCER2_TX1, MOTO_LDANCER2_TX0);
//Motor_ReadCfg(FPGA1, MOTO_LPIVOT1_TX1, MOTO_LPIVOT1_TX0);
Motor_Run(FPGA1, MOTO_WINDER_TX1, MOTO_WINDER_TX0, 800, 0x1);
//Thread.Sleep(30000);
//Motor_HighZ(FPGA1, MOTO_SCREW_TX1, MOTO_SCREW_TX0);
/*
Motor_Move_Loop(FPGA1, MOTO_LLOADING_TX1, MOTO_LLOADING_TX0, 0x3200, 0x1, 4);
Motor_HighZ(FPGA1, MOTO_LLOADING_TX1, MOTO_LLOADING_TX0);
Motor_Move_Loop(FPGA1, MOTO_LDANCER1_TX1, MOTO_LDANCER1_TX0, 0x3200, 0x1, 4);
Motor_HighZ(FPGA1, MOTO_LDANCER1_TX1, MOTO_LDANCER1_TX0);
// Motor_Move_Loop(FPGA1, MOTO_LDANCER2_TX1, MOTO_LDANCER2_TX0, 0x3200, 0x1, 4);
// Motor_HighZ(FPGA1, MOTO_LDANCER2_TX1, MOTO_LDANCER2_TX0);
Motor_Move_Loop(FPGA1, MOTO_LPIVOT1_TX1, MOTO_LPIVOT1_TX0, 0x3200, 0x1, 4);
Motor_HighZ(FPGA1, MOTO_LPIVOT1_TX1, MOTO_LPIVOT1_TX0);
*/
}
/* Script outputs motor driver chip config register
FPGA Reg. Read (FPGA Base, Addr, Data): (#60000000, #0226, #002E)
FPGA Reg. Read (FPGA Base, Addr, Data): (#60000000, #0224, #8800)
FPGA Reg. Read (FPGA Base, Addr, Data): (#60000000, #0236, #002E)
FPGA Reg. Read (FPGA Base, Addr, Data): (#60000000, #0234, #8800)
FPGA Reg. Read (FPGA Base, Addr, Data): (#60000000, #02C6, #002E)
FPGA Reg. Read (FPGA Base, Addr, Data): (#60000000, #02C4, #8800)
FPGA Reg. Read (FPGA Base, Addr, Data): (#60000000, #02D6, #002E)
FPGA Reg. Read (FPGA Base, Addr, Data): (#60000000, #02D4, #8800)
FPGA Reg. Read (FPGA Base, Addr, Data): (#60000000, #0306, #002E)
FPGA Reg. Read (FPGA Base, Addr, Data): (#60000000, #0304, #8800)
*/
|
