Mercurial > circuitpython-spiderwing

annotate python/servos.py @ 4:48cead9d49b5 draft default tip

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
Add a license
author Radomir Dopieralski <sheep-devel@sheep.art.pl>
date Sun, 23 Jul 2017 20:25:33 +0200
parents cf2cd5a35d24
children
Ignore whitespace changes - Everywhere: Within whitespace: At end of lines:
rev   line source
0
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
1 import pca9685
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
2 import math
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
3
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
4
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
5 TRIMS = [
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
6 0, -15, 0, # front left
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
7 0, 0,
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
8 10, -20, 0, # hind left
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
9 0, -10, .0, # hind right
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
10 0, 0,
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
11 0, -5, 0, # front right
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
12 ]
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
13
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
14
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
15 class Servos:
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
16 def __init__(self, i2c, address=0x40, freq=50, min_us=600, max_us=2400,
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
17 degrees=180):
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
18 self.period = 1000000 / freq
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
19 self.min_duty = self._us2duty(min_us)
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
20 self.max_duty = self._us2duty(max_us)
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
21 self.degrees = degrees
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
22 self.freq = freq
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
23 self.pca9685 = pca9685.PCA9685(i2c, address)
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
24 self.pca9685.freq(freq)
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
25
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
26 def _us2duty(self, value):
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
27 return int(4095 * value / self.period)
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
28
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
29 def position(self, index, degrees=None, radians=None, us=None, duty=None):
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
30 span = self.max_duty - self.min_duty
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
31 if degrees is not None:
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
32 duty = self.min_duty + span * degrees / self.degrees
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
33 elif radians is not None:
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
34 duty = self.min_duty + span * radians / math.radians(self.degrees)
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
35 elif us is not None:
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
36 duty = self._us2duty(us)
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
37 elif duty is not None:
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
38 pass
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
39 else:
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
40 return self.pca9685.duty(index)
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
41 duty = min(self.max_duty, max(self.min_duty, int(duty + TRIMS[index])))
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
42 self.pca9685.duty(index, duty)
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
43
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
44 def release(self, index):
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
45 self.pca9685.duty(index, 0)