Bang4SimPyLC/control.py at main · JEGGEgit/Bang4SimPyLC · 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
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
'''
====== Legal notices

Copyright (C) 2013 - 2021 GEATEC engineering

This program is free software.
You can use, redistribute and/or modify it, but only under the terms stated in the QQuickLicense.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY, without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
See the QQuickLicense for details.

The QQuickLicense can be accessed at: http://www.qquick.org/license.html

__________________________________________________________________________


 THIS PROGRAM IS FUNDAMENTALLY UNSUITABLE FOR CONTROLLING REAL SYSTEMS !!

__________________________________________________________________________

It is meant for training purposes only.

Removing this header ends your license.
'''

import simpylc as sp

class Control (sp.Module):
    def __init__ (self):
        sp.Module.__init__ (self)

        self.page ('movement control')          # is page1: on window1 @mje

        self.group ('torso drive control', True)
        self.torVoltFac = sp.Register (0.8)
        self.torVoltMax = sp.Register (10)
        self.torVolt = sp.Register ()
        self.torVoltSet = sp.Register ()
        self.torEnab = sp.Marker ()

        self.group ('torso angle')
        self.torAngSet = sp.Register ()
        self.torAng = sp.Register ()
        self.torAngDif = sp.Register ()
        self.torAngMid = sp.Register ()         # bang-bang settings @mje
        self.torAngMar = sp.Register ()         # target tolerance with direction
        self.torMarg = sp.Register (0.5)        # target tolerance
        self.torRound = sp.Marker ()            # is on target
        self.goPulse = sp.Oneshot ()            # movement restarted
        self.torAngChg = sp.Oneshot ()          # target has changed
        self.torAngSetOld = sp.Register ()

        self.group ('upper arm drive control', True)
        self.uppVoltFac = sp.Register (0.25)
        self.uppVoltMax = sp.Register (10)
        self.uppVolt = sp.Register ()
        self.uppEnab = sp.Marker ()

        self.group ('upper arm angle')
        self.uppAngSet = sp.Register ()
        self.uppAng = sp.Register ()
        self.uppAngOld = sp.Register ()
        self.uppAngDif = sp.Register ()
        self.uppMarg = sp.Register (15)
        self.uppRound = sp.Marker ()
        self.uppSpeedFac = sp.Register (0.5)
        self.uppSpeedMax = sp.Register (20)
        self.uppSpeedSet = sp.Register ()
        self.uppSpeed = sp.Register ()
        self.uppSpeedDif = sp.Register ()

        self.group ('fore arm drive control', True)
        self.forVoltFac = sp.Register (0.25)
        self.forVoltMax = sp.Register (10)
        self.forVolt = sp.Register ()
        self.forEnab = sp.Marker ()

        self.group ('fore arm angle')
        self.forAngSet = sp.Register ()
        self.forAng = sp.Register ()
        self.forAngOld = sp.Register ()
        self.forAngDif = sp.Register ()
        self.forMarg = sp.Register (15)
        self.forRound = sp.Marker ()
        self.forSpeedFac = sp.Register (0.5)
        self.forSpeedMax = sp.Register (20)
        self.forSpeedSet = sp.Register ()
        self.forSpeed = sp.Register ()
        self.forSpeedDif = sp.Register ()

        self.group ('wrist drive control', True)
        self.wriVoltFac = sp.Register (0.25)
        self.wriVoltMax = sp.Register (10)
        self.wriVolt = sp.Register ()
        self.wriEnab = sp.Marker ()

        self.group ('wrist angle')
        self.wriAngSet = sp.Register ()
        self.wriAng = sp.Register ()
        self.wriAngOld = sp.Register ()
        self.wriAngDif = sp.Register ()
        self.wriMarg = sp.Register (3)
        self.wriRound = sp.Marker ()
        self.wriSpeedFac = sp.Register (0.5)
        self.wriSpeedMax = sp.Register (20)
        self.wriSpeedSet = sp.Register ()
        self.wriSpeed = sp.Register ()
        self.wriSpeedDif = sp.Register ()

        self.group ('hand and fingers setpoints', True)
        self.hanAngSet = sp.Register ()
        self.hanEnab = sp.Marker ()
        self.finAngSet = sp.Register ()
        self.finEnab = sp.Marker ()
        self.finDelay = sp.Register (1)
        self.finTimer = sp.Timer ()
        self.finLatch = sp.Latch ()

        self.group ('sweep time measurement')
        self.sweepMin = sp.Register (1000)
        self.sweepMax = sp.Register ()
        self.sweepWatch = sp.Timer ()
        self.run = sp.Runner ()

        self.group ('general')
        self.go = sp.Marker ()

    def input (self):
        self.part ('true angles')
        self.torAng.set (sp.world.robot.torAng)
        self.uppAng.set (sp.world.robot.uppAng)
        self.forAng.set (sp.world.robot.forAng)
        self.wriAng.set (sp.world.robot.wriAng)
        self.torAngSet.set (sp.world.sequence.torAngSet) # from sequence @mje

    def sweep (self):
        self.part ('torso')
        self.torAngDif.set (self.torAngSet - self.torAng)
        self.torRound.mark (sp.abs (self.torAngDif) < self.torMarg)
        # bang-bang
        self.goPulse.trigger (self.go)
        self.torAngChg.trigger ((self.torAngSet != self.torAngSetOld) or self.goPulse)
        self.torAngMar.set (self.torMarg, self.torAng < self.torAngSet, -self.torMarg)
        self.torAngMid.set ((self.torAngSet + self.torAng) / 2 + self.torAngMar, self.torAngChg)
        self.torVoltSet.set (-self.torVoltMax, self.torAngMid < self.torAng, self.torVoltMax)
        self.torVolt.set (self.torVoltSet, not self.torRound, 0)  # Torq. ref. robot
        self.torEnab.mark (self.go and not self.torRound)         # Brake robot
        self.torAngSetOld.set (self.torAngSet)

        self.part ('upper arm')
        self.uppAngDif.set (self.uppAngSet - self.uppAng)
        self.uppRound.mark (sp.abs (self.uppAngDif) < self.uppMarg)
        self.uppSpeedSet.set (sp.limit (self.uppSpeedFac * self.uppAngDif, self.uppSpeedMax))
        self.uppSpeed.set ((self.uppAng - self.uppAngOld) / sp.world.period)
        self.uppSpeedDif.set (self.uppSpeedSet - self.uppSpeed)
        self.uppVolt.set (sp.limit (self.uppVoltFac * self.uppSpeedDif, self.uppVoltMax))
        # self.uppEnab.mark (self.go and self.torRound)
        self.uppEnab.mark (self.go)                             # simultan. @mje
        self.uppAngOld.set (self.uppAng)

        self.part ('fore arm')
        self.forAngDif.set (self.forAngSet - self.forAng)
        self.forRound.mark (sp.abs (self.forAngDif) < self.forMarg)
        self.forSpeedSet.set (sp.limit (self.forSpeedFac * self.forAngDif, self.forSpeedMax))
        self.forSpeed.set ((self.forAng - self.forAngOld) / sp.world.period)
        self.forSpeedDif.set (self.forSpeedSet - self.forSpeed)
        self.forVolt.set (sp.limit (self.forVoltFac * self.forSpeedDif, self.forVoltMax))
        self.forEnab.mark (self.go and self.torRound and self.uppRound)
        self.forAngOld.set (self.forAng)

        self.part ('wrist')
        self.wriAngDif.set (self.wriAngSet - self.wriAng)
        self.wriRound.mark (sp.abs (self.wriAngDif) < self.wriMarg)
        self.wriSpeedSet.set (sp.limit (self.wriSpeedFac * self.wriAngDif, self.wriSpeedMax))
        self.wriSpeed.set ((self.wriAng - self.wriAngOld) / sp.world.period)
        self.wriSpeedDif.set (self.wriSpeedSet - self.wriSpeed)
        self.wriVolt.set (sp.limit (self.wriVoltFac * self.wriSpeedDif, self.wriVoltMax))
        self.wriEnab.mark (self.go and self.torRound and self.uppRound and self.forRound)
        self.wriAngOld.set (self.wriAng)

        self.part ('hand and fingers')
        self.hanEnab.mark (self.go and self.torRound and self.uppRound and self.forRound and self.wriRound)
        self.finTimer.reset (not self.hanEnab)
        self.finEnab.mark (self.finTimer > self.finDelay)
        self.finLatch.latch (self.finTimer > 0.01)

        self.part ('sweep time measurement')
        self.sweepMin.set (sp.world.period, sp.world.period < self.sweepMin)
        self.sweepMax.set (sp.world.period, sp.world.period > self.sweepMax)
        self.sweepWatch.reset (self.sweepWatch > 2)
        self.sweepMin.set (1000, not self.sweepWatch)
        self.sweepMax.set (0, not self.sweepWatch)