def __init__(self, name, active,
horizontal_pin, vertical_pin,
hmin, hmax, vmin, vmax,
port, offset,
height,
horient, vorient):
self.name = name
self.active = active
self.hpin = horizontal_pin
self.vpin = vertical_pin
self.hmin = hmin
self.hmax = hmax
self.vmin = vmin
self.vmax = vmax
self.port = port
if self.active:
self.hservo = Servo(self.hpin, self.hmin, self.hmax, port)
self.vservo = Servo(self.vpin, self.vmin, self.vmax, port)
else:
self.hservo = None
self.vservo = None
self.offset = offset
self.height = height
self.horient = horient
self.vorient = vorient
self.move(*self.center())
self.focus = self.focus_simple
#self.focus = self.focus_angles
if cfg.general.debug:
print self.name, self.hpin, self.vpin, self.port, self.offset
print self.hservo
print self.vservo
class Eye(object):
def __init__(self, name, active,
horizontal_pin, vertical_pin,
hmin, hmax, vmin, vmax,
port, offset,
height,
horient, vorient):
self.name = name
self.active = active
self.hpin = horizontal_pin
self.vpin = vertical_pin
self.hmin = hmin
self.hmax = hmax
self.vmin = vmin
self.vmax = vmax
self.port = port
if self.active:
self.hservo = Servo(self.hpin, self.hmin, self.hmax, port)
self.vservo = Servo(self.vpin, self.vmin, self.vmax, port)
else:
self.hservo = None
self.vservo = None
self.offset = offset
self.height = height
self.horient = horient
self.vorient = vorient
self.move(*self.center())
self.focus = self.focus_simple
#self.focus = self.focus_angles
if cfg.general.debug:
print self.name, self.hpin, self.vpin, self.port, self.offset
print self.hservo
print self.vservo
def __unicode__(self):
return self.name
def settings(self):
print 'name:', self.name
print 'active:', self.active
print 'hpin:', self.hpin
print 'vpin:', self.vpin
print 'hmin:', self.hmin
print 'hmax:', self.hmax
print 'vmin:', self.vmin
print 'vmax:', self.vmax
print 'port:', self.port
print 'hservo:', self.hservo
print 'vservo:', self.vservo
print 'offset:', self.offset
print 'height:', self.height
print 'horient:', self.horient
print 'vorient:', self.vorient
def center(self):
return self.hmin + ((self.hmax - self.hmin) / 2.0), self.vmin + ((self.vmax - self.vmin) / 2.0)
def move(self, horizontal, vertical):
if self.active:
self.hservo.send(horizontal)
self.vservo.send(vertical)
def focus_simple(self, distance, point):
"""distance in meters.
point is x, y tuple for location in grid space.
angle of focus is currently very simplistic. it doesn't yet
take into account that the eyes are offset from the center and
it doesn't yet take into account the distance of the point in
question.
calculating the angle of focus depends on how the servo is
oriented and moves. if a low value moves the servo to the left
or up, then the calculation is:
range of movement * percentage of x as related to the
scale of the axis
if a high value moves the servo to the left or up, then the
calculation is:
axis max - range of movement * percentage of x as related
to scale of the axis
"""
xpercent = (self.hmax - self.hmin) * (point[0] / float(cfg.kinect.x))
ypercent = (self.vmax - self.vmin) * (point[1] / float(cfg.kinect.y))
if self.horient == 1: # movement scale goes from high to low
x = self.hmin + xpercent
else: # movement scale goes from low to high
x = self.hmax - xpercent
if self.vorient == 1: # movement scale goes from high to low
y = self.vmin + ypercent
else: # movement scale goes from low to high
y = self.vmax - ypercent
if cfg.general.debug:
print self.name, distance, point, x, y
self.move(x, y)
def focus_angles(self, distance, point):
"""distance in meters.
point is x, y tuple for location in grid space.
to calculate the focus for an eye on the x axis:
1) calculate the distance of the known point (px, py) to the center (cx, cy):
radius = sqrt(((px - cx) ** 2) + ((py - cy) ** 2))
where:
cx, cy = 640 / 2, 480 / 2
2) calculate the distance in pixels from the kinect to the center of the
640x480 field:
x_to_center = 320 / tan(kinect_field_x / 2)
where:
320 is 1/2 of the horizontal field (sets up for a right triangle)
kinect_field_x is the kinect's field of vision along the x axis.
3) x_angle = atan(radius / x_to_center)
for the y axis:
1) same as for x axis
2) y_to_center = 240 / tan(kinect_field_y / 2)
where 240 is 1/2 of the vertical field and kinect_field_y is the kinect's
field of vision along the y axis.
3) y_angle = atan(radius / y_to_center)
"""
radius = math.sqrt(((point[0] - 320) ** 2) + ((point[1] - 240) ** 2))
x_to_center = 320 / math.tan(math.radians(cfg.kinect.x / 2))
x_angle = math.degrees(math.atan(radius / x_to_center))
y_to_center = 240 / math.tan(math.radians(cfg.kinect.y / 2))
y_angle = math.degrees(math.atan(radius / y_to_center))
if self.horient == 1: # movement scale goes from high to low
x = 90 - x_angle
else: # movement scale goes from low to high
x = 90 + x_angle
if self.vorient == 1: # movement scale goes from high to low
y = 90 - y_angle
else: # movement scale goes from low to high
y = 90 + y_angle
if cfg.general.debug:
print distance, point, x, y, radius, x_angle, y_angle, self.horient
self.move(x, y)
