Rev 4423 Rev 4451
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 import numpy as np
13 from pymlab import config 13 from pymlab import config
14   14  
15 #### Script Arguments ############################################### 15 #### Script Arguments ###############################################
16   16  
17 if len(sys.argv) != 2: 17 if len(sys.argv) != 2:
18 sys.stderr.write("Invalid number of arguments.\n") 18 sys.stderr.write("Invalid number of arguments.\n")
19 sys.stderr.write("Usage: %s PORT ADDRESS\n" % (sys.argv[0], )) 19 sys.stderr.write("Usage: %s PORT ADDRESS\n" % (sys.argv[0], ))
20 sys.exit(1) 20 sys.exit(1)
21   21  
22 port = eval(sys.argv[1]) 22 port = eval(sys.argv[1])
23 #### Sensor Configuration ########################################### 23 #### Sensor Configuration ###########################################
24   24  
25 '''' 25 ''''
26 cfg = config.Config( 26 cfg = config.Config(
27 i2c = { 27 i2c = {
28 "port": port, 28 "port": port,
29 }, 29 },
30   30  
31 bus = [ 31 bus = [
32 { 32 {
33 "type": "i2chub", 33 "type": "i2chub",
34 "address": 0x72, 34 "address": 0x72,
35 35
36 "children": [ 36 "children": [
37 {"name": "encoder", "type": "rps01", "channel": 1, } 37 {"name": "encoder", "type": "rps01", "channel": 1, }
38 ], 38 ],
39 }, 39 },
40 ], 40 ],
41 ) 41 )
42   42  
43 ''' 43 '''
44 cfg = config.Config( 44 cfg = config.Config(
45 i2c = { 45 i2c = {
46 "port": port, 46 "port": port,
47 }, 47 },
48 bus = [ 48 bus = [
49 { 49 {
50 "name": "encoder", 50 "name": "encoder",
51 "type": "rps01", 51 "type": "rps01",
52 }, 52 },
53 ], 53 ],
54 ) 54 )
55   55  
56   56  
57 cfg.initialize() 57 cfg.initialize()
58   58  
59 print "RPS01A magnetic position sensor RPS01 readout example \r\n" 59 print "RPS01A magnetic position sensor RPS01 readout example \r\n"
60 sensor = cfg.get_device("encoder") 60 sensor = cfg.get_device("encoder")
61   61  
62 print sensor.get_address() 62 print sensor.get_address()
63 print sensor.get_zero_position() 63 print sensor.get_zero_position()
64   64  
65 #### Data Logging ################################################### 65 #### Data Logging ###################################################
66   66  
67 try: 67 try:
68 angles = np.zeros(5) 68 angles = np.zeros(5)
69 angles[4] = sensor.get_angle(verify = False) 69 angles[4] = sensor.get_angle(verify = False)
70 time.sleep(0.1) 70 time.sleep(0.01)
71 angles[3] = sensor.get_angle(verify = False) 71 angles[3] = sensor.get_angle(verify = False)
72 time.sleep(0.1) 72 time.sleep(0.01)
73 angles[2] = sensor.get_angle(verify = False) 73 angles[2] = sensor.get_angle(verify = False)
74 time.sleep(0.1) 74 time.sleep(0.01)
75 angles[1] = sensor.get_angle(verify = False) 75 angles[1] = sensor.get_angle(verify = False)
76 n = 0 76 n = 0
-   77 speed = 0
-   78 AVERAGING = 50
77   79  
78 while True: 80 while True:
-   81 for i in range(AVERAGING):
79 time.sleep(0.1) 82 time.sleep(0.01)
80 angles[0] = sensor.get_angle(verify = False) 83 angles[0] = sensor.get_angle(verify = False)
81 84
82 if (angles[0] + n*360 - angles[1]) > 300: 85 if (angles[0] + n*360 - angles[1]) > 300:
83 n -= 1 86 n -= 1
84 angles[0] = angles[0] + n*360 87 angles[0] = angles[0] + n*360
-   88  
85 elif -(angles[0] - n*360 - angles[1]) > 300: # compute angular speed in backward direction. 89 elif (angles[0] + n*360 - angles[1]) < -300: # compute angular speed in backward direction.
86 n += 1 90 n += 1
87 angles[0] = angles[0] - n*360 91 angles[0] = angles[0] + n*360
-   92  
88 else: 93 else:
89 angles[0] = angles[0] + n*360 94 angles[0] = angles[0] + n*360
90 95
91 speed = (-angles[4] + 8*angles[3] - 8*angles[1] + angles[0])/12 96 speed += (-angles[4] + 8*angles[3] - 8*angles[1] + angles[0])/12
92 angles = np.roll(angles, 1) 97 angles = np.roll(angles, 1)
93 98  
94 sys.stdout.write("Speed: " + str(speed) +"\t"+ str(angles[0]) + "\r\n") 99 speed = speed/AVERAGING # apply averaging on acummulated value.
95 sys.stdout.flush() 100 print "Speed: %0.2f \t Total Angle: %0.2f \r\n" % (speed, angles[0])
-   101  
96 except KeyboardInterrupt: 102 except KeyboardInterrupt:
97 sys.exit(0) 103 sys.exit(0)