1 |
#!/usr/bin/python |
1 |
#!/usr/bin/python |
2 |
|
2 |
|
3 |
# Python library for RPS01A MLAB module with AS5048B I2C Magnetic position sensor. |
3 |
# Python library for RPS01A MLAB module with AS5048B I2C Magnetic position sensor. |
4 |
|
4 |
|
5 |
#uncomment for debbug purposes |
5 |
#uncomment for debbug purposes |
6 |
#import logging |
6 |
#import logging |
7 |
#logging.basicConfig(level=logging.DEBUG) |
7 |
#logging.basicConfig(level=logging.DEBUG) |
8 |
|
8 |
|
9 |
import time |
9 |
import time |
10 |
import datetime |
10 |
import datetime |
11 |
import sys |
11 |
import sys |
- |
|
12 |
import numpy as np |
12 |
from pymlab import config |
13 |
from pymlab import config |
13 |
|
14 |
|
14 |
#### Script Arguments ############################################### |
15 |
#### Script Arguments ############################################### |
15 |
|
16 |
|
16 |
if len(sys.argv) != 2: |
17 |
if len(sys.argv) != 2: |
17 |
sys.stderr.write("Invalid number of arguments.\n") |
18 |
sys.stderr.write("Invalid number of arguments.\n") |
18 |
sys.stderr.write("Usage: %s PORT ADDRESS\n" % (sys.argv[0], )) |
19 |
sys.stderr.write("Usage: %s PORT ADDRESS\n" % (sys.argv[0], )) |
19 |
sys.exit(1) |
20 |
sys.exit(1) |
20 |
|
21 |
|
21 |
port = eval(sys.argv[1]) |
22 |
port = eval(sys.argv[1]) |
22 |
#### Sensor Configuration ########################################### |
23 |
#### Sensor Configuration ########################################### |
23 |
|
24 |
|
24 |
'''' |
25 |
'''' |
25 |
cfg = config.Config( |
26 |
cfg = config.Config( |
26 |
i2c = { |
27 |
i2c = { |
27 |
"port": port, |
28 |
"port": port, |
28 |
}, |
29 |
}, |
29 |
|
30 |
|
30 |
bus = [ |
31 |
bus = [ |
31 |
{ |
32 |
{ |
32 |
"type": "i2chub", |
33 |
"type": "i2chub", |
33 |
"address": 0x72, |
34 |
"address": 0x72, |
34 |
|
35 |
|
35 |
"children": [ |
36 |
"children": [ |
36 |
{"name": "encoder", "type": "rps01", "channel": 1, } |
37 |
{"name": "encoder", "type": "rps01", "channel": 1, } |
37 |
], |
38 |
], |
38 |
}, |
39 |
}, |
39 |
], |
40 |
], |
40 |
) |
41 |
) |
41 |
|
42 |
|
42 |
''' |
43 |
''' |
43 |
cfg = config.Config( |
44 |
cfg = config.Config( |
44 |
i2c = { |
45 |
i2c = { |
45 |
"port": port, |
46 |
"port": port, |
46 |
}, |
47 |
}, |
47 |
bus = [ |
48 |
bus = [ |
48 |
{ |
49 |
{ |
49 |
"name": "encoder", |
50 |
"name": "encoder", |
50 |
"type": "rps01", |
51 |
"type": "rps01", |
51 |
}, |
52 |
}, |
52 |
], |
53 |
], |
53 |
) |
54 |
) |
54 |
|
55 |
|
55 |
|
56 |
|
56 |
cfg.initialize() |
57 |
cfg.initialize() |
57 |
|
58 |
|
58 |
print "RPS01A magnetic position sensor RPS01 readout example \r\n" |
59 |
print "RPS01A magnetic position sensor RPS01 readout example \r\n" |
59 |
sensor = cfg.get_device("encoder") |
60 |
sensor = cfg.get_device("encoder") |
60 |
|
61 |
|
61 |
print sensor.get_address() |
62 |
print sensor.get_address() |
62 |
print sensor.get_zero_position() |
63 |
print sensor.get_zero_position() |
63 |
|
64 |
|
64 |
#### Data Logging ################################################### |
65 |
#### Data Logging ################################################### |
65 |
|
66 |
|
66 |
try: |
67 |
try: |
67 |
while True: |
68 |
angles = np.zeros(5) |
- |
|
69 |
angles[4] = sensor.get_angle(verify = False) |
68 |
# for i in range(10): |
70 |
time.sleep(0.1) |
69 |
angle1 = sensor.get_angle(verify = False) |
71 |
angles[3] = sensor.get_angle(verify = False) |
70 |
time.sleep(0.1) |
72 |
time.sleep(0.1) |
71 |
angle2 = sensor.get_angle(verify = False) |
73 |
angles[2] = sensor.get_angle(verify = False) |
72 |
time.sleep(0.1) |
74 |
time.sleep(0.1) |
73 |
angle3 = sensor.get_angle(verify = False) |
75 |
angles[1] = sensor.get_angle(verify = False) |
- |
|
76 |
n = 0 |
74 |
|
77 |
|
75 |
if (angle1 < angle2): |
78 |
while True: |
76 |
speed = (angle2 - angle1)/0.01 |
- |
|
77 |
else: |
79 |
time.sleep(0.1) |
78 |
speed = (360 - angle1 + angle2)/0.01 |
80 |
angles[0] = sensor.get_angle(verify = False) |
79 |
|
81 |
|
- |
|
82 |
if (angles[0] + n*360 - angles[1]) > 300: |
- |
|
83 |
n -= 1 |
- |
|
84 |
angles[0] = angles[0] + n*360 |
- |
|
85 |
elif -(angles[0] - n*360 - angles[1]) > 300: # compute angular speed in backward direction. |
- |
|
86 |
n += 1 |
- |
|
87 |
angles[0] = angles[0] - n*360 |
- |
|
88 |
else: |
- |
|
89 |
angles[0] = angles[0] + n*360 |
80 |
|
90 |
|
- |
|
91 |
speed = (-angles[4] + 8*angles[3] - 8*angles[1] + angles[0])/12 |
- |
|
92 |
angles = np.roll(angles, 1) |
81 |
|
93 |
|
82 |
sys.stdout.write("Speed: " + str(speed) +"\t"+ str(angle1) +"\t"+ str(angle2) + "\t\tMagnitude: " + str(sensor.get_magnitude()) |
94 |
sys.stdout.write("Speed: " + str(speed) +"\t"+ str(angles[0]) + "\r\n") |
83 |
+ "\tAGC Value: " + str(sensor.get_agc_value()) + "\tDiagnostics: " + str(sensor.get_diagnostics()) + "\r\n") |
- |
|
84 |
sys.stdout.flush() |
95 |
sys.stdout.flush() |
85 |
time.sleep(0.01) |
- |
|
86 |
except KeyboardInterrupt: |
96 |
except KeyboardInterrupt: |
87 |
sys.exit(0) |
97 |
sys.exit(0) |