pythoncode/MotionController.py at master · lesagejonathan/pythoncode · GitHub

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
import gclib
import time

class Controller:

    def __init__(self,instrument='ImmersionTank'):


        if instrument is 'ImmersionTank':

            self.AxisKeys = {'X':'A','Y':'B','Z':'C','Rotation':'D','E':'Yaw','F':'Pitch'}

            self.StepsPerMeasure = {'X':1000.,'Y':801.721016,'Z':1000.,'Rotation':160.*1000./360.,'Pitch':266.666666667,'Yaw':1310.3}

            ip = '10.10.1.12'

        elif instrument is 'ZMC4':

            self.AxisKeys = {'Rotation':'A', 'Index':'B'}

            self.StepsPerMeasure = {'Index':131345. , 'Rotation':79.6}

            ip = '192.168.1.250'


        self.Galil = gclib.py()

        self.Galil.GOpen(ip+' --direct -s ALL')

        self.CurrentPosition = {}


        for k in list(self.AxisKeys.keys()):

            self.ZeroEncoder(k)

    def Wait(self):

        inmotion = True

        while inmotion:

            axisinmotion = [self.CheckMotionComplete(axis) for axis in list(self.AxisKeys.keys())]

            inmotion = not(all(axisinmotion))

    def MoveRelative(self,Axis,Position,Speed, Wait=False):
        # set Speed

        if Wait:

            self.Wait()


        self.Galil.GCommand('SH'+self.AxisKeys[Axis])

        self.Galil.GCommand('SP'+self.AxisKeys[Axis]+'='+str(round(Speed*self.StepsPerMeasure[Axis])))

        # set Position

        self.Galil.GCommand('PR'+self.AxisKeys[Axis]+'='+str(round(Position*self.StepsPerMeasure[Axis])))

        # execute Move

        self.Galil.GCommand('BG'+self.AxisKeys[Axis])


        # Reset Global Position

        self.CurrentPosition[Axis] += Position

    def MoveAbsolute(self,Axis,Position,Speed,Wait=False):

        if Wait:

            self.Wait()

        # set Speed

        self.Galil.GCommand('SH'+self.AxisKeys[Axis])

        self.Galil.GCommand('SP'+self.AxisKeys[Axis]+'='+str(round(Speed*self.StepsPerMeasure[Axis])))

        # set Position

        self.Galil.GCommand('PA'+self.AxisKeys[Axis]+'='+str(round(Position*self.StepsPerMeasure[Axis])))

        # execute Move

        self.Galil.GCommand('BG'+self.AxisKeys[Axis])

        # Reset Global Position

        self.CurrentPosition[Axis] = Position

    def ZeroEncoder(self,Axis):

        self.Galil.GCommand('DP'+self.AxisKeys[Axis]+'=0')
        self.CurrentPosition[Axis] = 0.0

    def CheckMotionComplete(self, Axis):

        return not(bool(int(float(self.Galil.GCommand('MG _BG'+self.AxisKeys[Axis])))))

    def MoveToLimit(self, Axis, Speed, Direction, Limit=5000):


        if Direction == 'Forward':

                self.MoveRelative(Axis,Limit,Speed,Wait=True)

                notlimit = True

                while notlimit:

                    notlimit = bool(int(float(self.Galil.GCommand('MG _LF'+self.AxisKeys[Axis]))))


        elif Direction == 'Backward':


                self.MoveRelative(Axis,-Limit,Speed,Wait=True)

                notlimit = True

                while notlimit:

                    notlimit = bool(int(float(self.Galil.GCommand('MG _LR'+self.AxisKeys[Axis]))))


    def __del__(self):

        self.Galil.GClose()