forked from gkanwar/maslab-staff-2013
-
Notifications
You must be signed in to change notification settings - Fork 0
/
ir_dist.py
91 lines (86 loc) · 3.62 KB
/
ir_dist.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
# Matt Vernacchia | MASLab Team 4
# defines a IR_Dist class for interacting with the IR Distance sensors. Example code a bottom of file.
import pickle
import arduino
import time
import numpy, pylab # for plotting
class IR_Dist:
def __init__(self, ardu, port, idString):
self.ain = arduino.AnalogInput(ardu, port)
self.vals = None
self.idString = idString
def load(self):
try:
f = open( ('ir_dist_calibration_' + self.idString + '.ircalib'), 'r')
self.vals = pickle.load(f)
print 'loaded IR distance calibration from ' + ('ir_dist_calibration_' + self.idString + '.ircalib')
print self.vals
except IOError as e:
print "Error reading from IR calibration file: {0} {1}".format(e.errno, e.strerror)
except:
print "unexpected error reading from IR sensor calibration file"
def getVolt(self):
"""Returns the analog volatage output of the IR sensor in the arduino's
voltage units (1 unit = 4.9mV, 1024 units = 5v)"""
return self.ain.getValue()
def getDist(self):
"""Returns the distance currently read by the IR distance sensor, in centimeters."""
return self.convertToDist( self.ain.getValue() )
def convertToDist(self, volt):
""" Converts a voltage reading to a distance based on the calibration data"""
if self.vals == None:
print "IR_Dist: must calibrate and load before converting voltage to distance"
self.load()
i = 0
while (i < (len(self.vals)-1)) and (self.vals[i][0] < volt):
i = i + 1
if i == 0: return self.vals[0][1]
elif i == (len(self.vals)-1): return self.vals[-1][1]
else:
# linear interpolation between the two closest calibrated values
ratio = (self.vals[i-1][0] - volt)/(self.vals[i-1][0] - self.vals[i][0])
return ( self.vals[i-1][1] + ratio*(self.vals[i][1]-self.vals[i-1][1]) )
def calibrate(self):
"""Manually calibrate the distance readings on the IR sensors and save the results to a file"""
vals = []
print ""
print "Manual Calibration of IR Distance Sensors"
print "Type 'q' to quit"
while True:
s = raw_input("Enter target distance in cm: ")
if s == "q": break
dist = int(s)
volt = 0
for x in range(10):
volt = volt + self.ain.getValue()
volt = volt/10.0
vals.append( (volt, dist) )
# sort the list of voltage-distance pairs
vals = sorted(vals)
# pickle the list of voltage-distance paris to a file
f = open( ('ir_dist_calibration_' + self.idString + '.ircalib'), 'w')
pickle.dump(vals, f)
def plotCalibrationData(self):
""" Plots a graph of the calibration data. blocks until the graph window is closed?"""
if self.vals == None: print "must calibrate and load before plotting"
volts = numpy.zeros( len(self.vals) )
dists = numpy.zeros( len(self.vals) )
for i in range( len(self.vals) ):
volts[i] = self.vals[i][0]
dists[i] = self.vals[i][1]
pylab.plot(dists, volts)
pylab.xlabel('Distance [cm]')
pylab.ylabel('Voltage [4.9mV steps]')
pylab.title('IR Distance Sensor Calibration Data')
pylab.show()
####### testing #########
def test():
ardu = arduino.Arduino()
ir = IR_Dist(ardu, 4, 'a')
ardu.run()
time.sleep(1)
ir.calibrate() # only need to do this once - the results are saved to disk
ir.load()
ir.plotCalibrationData()
while True:
print ir.getDist()