Rev 4413 Rev 4423
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:
-   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  
67 while True: 78 while True:
68 # for i in range(10): -  
69 angle1 = sensor.get_angle(verify = False) -  
70 time.sleep(0.1) -  
71 angle2 = sensor.get_angle(verify = False) -  
72 time.sleep(0.1) 79 time.sleep(0.1)
73 angle3 = sensor.get_angle(verify = False) 80 angles[0] = sensor.get_angle(verify = False)
74 81
-   82 if (angles[0] + n*360 - angles[1]) > 300:
-   83 n -= 1
75 if (angle1 < angle2): 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
76 speed = (angle2 - angle1)/0.01 87 angles[0] = angles[0] - n*360
77 else: 88 else:
78 speed = (360 - angle1 + angle2)/0.01 89 angles[0] = angles[0] + n*360
79 -  
80 -  
81 90
82 sys.stdout.write("Speed: " + str(speed) +"\t"+ str(angle1) +"\t"+ str(angle2) + "\t\tMagnitude: " + str(sensor.get_magnitude()) 91 speed = (-angles[4] + 8*angles[3] - 8*angles[1] + angles[0])/12
-   92 angles = np.roll(angles, 1)
-   93
83 + "\tAGC Value: " + str(sensor.get_agc_value()) + "\tDiagnostics: " + str(sensor.get_diagnostics()) + "\r\n") 94 sys.stdout.write("Speed: " + str(speed) +"\t"+ str(angles[0]) + "\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)