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