/
servo.py
201 lines (166 loc) · 6.92 KB
/
servo.py
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
198
199
200
201
# -*- coding: utf-8 -*-
from __future__ import division
import sys
from Adafruit_PWM_Servo_Driver import PWM
class Servo(object):
def __init__(self, i2caddress, channel, pwm_min, pwm_max, reversed=False, offset=0, minangle=-90, maxangle=90,
callback=None):
# TODO: memorize max and min positions and set functions reaching it
self.i2caddress = i2caddress
self._pwm = PWM(self.i2caddress, debug=False) # Set debug to False before release
self.channel = channel
self._pwm.setPWMFreq(60)
self.pwm_min = pwm_min # pwm_min: HS-485HB->158 HS-645MG->149
self.pwm_max = pwm_max # pwm_max: HS-485HB->643 HS-645MG->651
self._actualpwm = None
self.angle = None
self.anglethrow = 180 # Maximum angle throw of servo end-to-end
self.offset = offset
self.reversed = reversed
self.callback = callback
self.powered = None
self.poweroff()
if maxangle - minangle > self.anglethrow:
sys.exit('Result of maxAngle-minAngle exceeds predefined servo throw of %s' % self.anglethrow)
elif not (self.setminangle(minangle) and self.setmaxangle(maxangle)):
sys.exit('Error initializing servo on channel #%s. Unable to set min/max values' % channel)
def _ispwmvalid(self, value):
if self.pwm_min <= value <= self.pwm_max:
return True
else:
print(
'PWM=%s is not valid. It is not between min/max pwm values of %s/%s' % (
value, self.pwm_min, self.pwm_max))
return False
def _isanglewithinlimits(self, angle):
# Servo's angle limit can be less than a valid pwm value
# This function limits servo angle amplitude
# If min/max angles are not set they assume easy to superate values
if not hasattr(self, 'minangle') or (self.minangle is None):
self.minangle = -999999
if not hasattr(self, 'maxangle') or self.maxangle is None:
self.maxangle = 999999
if self.minangle <= angle <= self.maxangle:
return True
else:
if angle < self.minangle: print(
'Angle:%sº is lower than defined min. angle:%s (channel#%s)' % (angle, self.minangle, self.channel))
if angle > self.maxangle: print(
'Angle:%sº is higher than defined max. angle:%s (channel#%s)' % (angle, self.maxangle, self.channel))
return False
def checkservoangle(self, angle):
# Assumes not for start
result = False
pwm = self._convangletopwm(angle)
if self._isanglewithinlimits(angle) and self._ispwmvalid(pwm):
result = True
return result
def _setpwm(self, value):
self._pwm.setPWM(self.channel, 0, value)
self._actualpwm = value
self.powered = True
# print('PWM set to: %s' % value)
def _convangletopwm(self, angle):
# return int(0.0022*pwm**2-2.416*pwm+500.42)
absangle = angle + 90
return int(((absangle * (self.pwm_max - self.pwm_min)) / self.anglethrow) + self.pwm_min)
def setminangle(self, angle):
if self.checkservoangle(angle):
self.minangle = angle
return True
else:
print('Impossible to set Min angle of channel #%s to %sº!' % (self.channel, angle))
return False
def setmaxangle(self, angle):
if self.checkservoangle(angle):
self.maxangle = angle
return True
else:
print('Impossible to set Max angle of channel #%s to %sº!' % (self.channel, angle))
return False
def calcanglefrominc(self, angleinc):
return self.angle + angleinc
def checkincangle(self, angleinc):
endangle = self.calcanglefrominc(angleinc)
return self.checkservoangle(endangle)
def setoffset(self, angleoffset):
# Sets offset angle. Returns True if offset successful
oldoffset = self.offset
self.offset = angleoffset
# Trying to update servo position according new offset angle
if self.setangle(self.angle):
print('Channel %s (%s)> offset: %s' % (self.channel, self.i2caddress, self.offset))
return True
else:
print('Error applying offsets')
self.offset = oldoffset
return False
def incoffset(self, angleoffset):
# Sets offset angle. Returns True if offset successful
oldoffset = self.offset
self.offset = self.offset + angleoffset
# Trying to update servo position according new offset angle
if self.setangle(self.angle):
print('Channel %s (%s)> offset: %s' % (self.channel, self.i2caddress, self.offset))
return True
else:
print('Error applying offsets')
self.offset = oldoffset
return False
@property
def getOffset(self):
return self.offset
def _calcoffsetangle(self, angle):
offsetangle = self.offset + angle
if self.reversed:
offsetangle = -offsetangle # Reverses angle in case of servo 'reversed'
return offsetangle
def setangle(self, angle):
# Executes order to move servo to specified angle
# Returns False if not possible or True if OK
offsetAngle = self._calcoffsetangle(angle)
if not self.checkservoangle(offsetAngle):
# Angle not within servo range
return False
else:
pwmvalue = self._convangletopwm(offsetAngle)
self._setpwm(pwmvalue)
self.angle = float(angle) # Sets the original non offseted angle >>CONFIRM
if self.callback is not None: self.callback()
return True
def setangletominormax(self, minmax):
# Sets servo to its min/max (-1/1) angle
if minmax == 1: # Max
print('Setting channel %s to Max' % self.channel)
if self.reversed:
angle = self.minangle
else:
angle = self.maxangle
else: # Min
print('Setting channel %s to Min' % self.channel)
if self.reversed:
angle = self.maxangle
else:
angle = self.minangle
if not self.checkservoangle(angle):
# Angle not within servo range
return False
else:
pwmvalue = self._convangletopwm(angle)
self._setpwm(pwmvalue)
self.angle = angle
if self.callback is not None: self.callback()
return True
def incangle(self, angleinc):
return self.setangle(self.calcanglefrominc(float(angleinc)))
@property
def getangle(self):
return self.angle
def poweroff(self):
self._setpwm(0)
self.powered = False
def poweron(self):
self.setangle(self.angle)
self.powered = True
def refresh(self):
return self.setangle(self.angle)