-
Notifications
You must be signed in to change notification settings - Fork 0
/
comedi_combo_test.py
93 lines (59 loc) · 2.55 KB
/
comedi_combo_test.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
# comedi_combo_test.py
import comedi as c
import time
channel = 0
comediDevice = c.comedi_open('/dev/comedi0')
print comediDevice
print "***** HANDLING INPUT DEVICE(S) *****"
inputSubdev = c.comedi_find_subdevice_by_type(comediDevice, c.COMEDI_SUBD_AI, 0)
print "input subdevice: %d" % inputSubdev
inputMaxdata = c.comedi_get_maxdata(comediDevice, inputSubdev, channel)
print "Input max Data: %d" % inputMaxdata
nInputChannels = c.comedi_get_n_channels(comediDevice, inputSubdev)
print "Num input Channels: %d" % nInputChannels
nInputRanges = c.comedi_get_n_ranges(comediDevice, inputSubdev, channel)
print "number Input Ranges: %d" % nInputRanges
inputRange = c.comedi_get_range(comediDevice, inputSubdev, channel, 0)
print "input range: : %s" % str(inputRange)
print "Input locked? %d" % c.comedi_lock(comediDevice, inputSubdev)
print "***** HANDLING OUTPUT DEVICE(S) *****"
outputSubdev = c.comedi_find_subdevice_by_type(comediDevice, c.COMEDI_SUBD_AO, 0)
print "output subdevice: %d" % outputSubdev
outputMaxdata = c.comedi_get_maxdata(comediDevice, outputSubdev, channel)
print "output max Data: %d" % outputMaxdata
nOutputChannels = c.comedi_get_n_channels(comediDevice, outputSubdev)
print "Num output Channels: %d" % nOutputChannels
nOutputRanges = c.comedi_get_n_ranges(comediDevice, outputSubdev, channel)
print "number output Ranges: %d" % nOutputRanges
outputRange = c.comedi_get_range(comediDevice, outputSubdev, channel, 0)
print "output range: : %s" % str(outputRange)
print "Output locked? %d" % c.comedi_lock(comediDevice, outputSubdev)
print "***** Done handling DEVICE(S), let's get 'er done! *****"
rdata = 0 # read data
wdata = outputMaxdata/2
theRange = 0
aref = 0
# start the motor at zero speed:
c.comedi_data_write(comediDevice, outputSubdev, channel, theRange, aref, wdata)
while True:
startTime = time.time()
ret, rdata = c.comedi_data_read(comediDevice, inputSubdev, channel, theRange, aref)
voltage = c.comedi_to_phys(rdata, inputRange, inputMaxdata);
#print "Voltage: %f" % voltage
x1 = voltage
x2 = x1*voltage
x3 = x2*voltage
x4 = x3*voltage
x5 = x4*voltage
dist = -14.153*x5+110.18*x4-339.89*x3+538.13*x2-479.23*x1+243.35
#print "Distance: %f" % dist
if dist > 50:
wdata = outputMaxdata/2 + 1000
else:
wdata = outputMaxdata/2
#print 'Writing %d to the motor' % wdata
c.comedi_data_write(comediDevice, outputSubdev, channel, theRange, aref, wdata)
print time.time() - startTime
#time.sleep(1)
print "Closing %s" % str(comediDevice)
c.comedi_close(comediDevice)