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)
4451 kaklik 70 time.sleep(0.01)
4423 kaklik 71 angles[3] = sensor.get_angle(verify = False)
4451 kaklik 72 time.sleep(0.01)
4423 kaklik 73 angles[2] = sensor.get_angle(verify = False)
4451 kaklik 74 time.sleep(0.01)
4423 kaklik 75 angles[1] = sensor.get_angle(verify = False)
76 n = 0
4451 kaklik 77 speed = 0
78 AVERAGING = 50
4423 kaklik 79  
4413 kaklik 80 while True:
4451 kaklik 81 for i in range(AVERAGING):
82 time.sleep(0.01)
83 angles[0] = sensor.get_angle(verify = False)
84  
85 if (angles[0] + n*360 - angles[1]) > 300:
86 n -= 1
87 angles[0] = angles[0] + n*360
88  
89 elif (angles[0] + n*360 - angles[1]) < -300: # compute angular speed in backward direction.
90 n += 1
91 angles[0] = angles[0] + n*360
92  
93 else:
94 angles[0] = angles[0] + n*360
95  
96 speed += (-angles[4] + 8*angles[3] - 8*angles[1] + angles[0])/12
97 angles = np.roll(angles, 1)
98  
99 speed = speed/AVERAGING # apply averaging on acummulated value.
100 print "Speed: %0.2f \t Total Angle: %0.2f \r\n" % (speed, angles[0])
101  
4413 kaklik 102 except KeyboardInterrupt:
103 sys.exit(0)