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
|
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;
Int32 last_14bit_position;
Int32 position_32bit=0;
const Int32 DrierLoadingArm_Motor = 5;
public void OnExecute(StubManager stubManager)
{
string FileName="test.csv";
var stopwatch = new Stopwatch();
stopwatch.Start();
long elapsed_time = stopwatch.ElapsedMilliseconds;
ReadEncoder_Dryer();
position_32bit=0;
ReadEncoder_Dryer();
Int32 temp=position_32bit;
stubManager.WriteToFile(FileName,DateTime.Now +" " );
//stubManager.AppendToFile("test.csv",elapsed_time+ ","+ position_32bit );
stubManager.Run<StubMotorRunResponse>("StubMotorRunRequest" ,DrierLoadingArm_Motor, false, 100);//40
elapsed_time = stopwatch.ElapsedMilliseconds;
stubManager.AppendToFile(FileName,elapsed_time+ ","+ position_32bit );
while (true)
{
ReadEncoder_Dryer();
elapsed_time = stopwatch.ElapsedMilliseconds;
stubManager.AppendToFile(FileName,elapsed_time+ ","+ position_32bit );
Thread.Sleep(1);
}
}
void ReadEncoder_Dryer()
{
Int32 temp;
Int32 temp1;
stubManager.Run<StubFpgaWriteRegResponse>("StubFpgaWriteRegRequest" ,0x6000019e, 0x0000); //triggers a TX transmission
var response = stubManager.Run<StubFpgaReadRegResponse>("StubFpgaReadRegRequest" ,0x60000190);
var response1 = stubManager.Run<StubFpgaReadRegResponse>("StubFpgaReadRegRequest" ,0x60000192);
temp = Convert.ToInt32 (((((response1.Value <<16) + response.Value)>>10 ) & 0x3fff));
if (temp>last_14bit_position)
{
temp1=temp-last_14bit_position;
position_32bit=position_32bit+temp1;
if (temp1>0x2000)
position_32bit=position_32bit-0x4000;
}
else if (temp<last_14bit_position)
{
temp1=last_14bit_position-temp;
position_32bit=position_32bit-temp1;
if (temp1>0x2000)
position_32bit=position_32bit+0x4000;
}
last_14bit_position=temp;
return ;
}
|
