def run(centerPan, centerTilt, radius, endTime):
print("circle running")
#choose a random start
currentAngle = random.randrange(0, 360)
#place the lazer on the circle
pantilthat.pan(findX(currentAngle, radius, centerPan))
pantilthat.tilt(findY(currentAngle, radius, centerTilt))
time.sleep(0.05)
while time.time() < endTime:
if (currentAngle > 360):
currentAngle = 0
currentAngle += 1
pan = findX(currentAngle, radius, centerPan)
tilt = findY(currentAngle, radius, centerTilt)
if (boundingPoly.inPanBounds(pan, tilt) == False):
if (pan > boundingPoly.getMaxPan(tilt)):
pan = boundingPoly.getMaxPan(tilt)
if (pan < boundingPoly.getMinPan(tilt)):
pan = boundingPoly.getMinPan(tilt)
if (boundingPoly.inTiltBounds(tilt, pan) == False):
if (tilt > boundingPoly.getMaxTilt(pan)):
tilt = boundingPoly.getMaxTilt(pan)
if (tilt < boundingPoly.getMinTilt(pan)):
tilt = boundingPoly.getMinTilt(pan)
#print ("currentAngle="+ str(currentAngle) + " pan=" + str(pan) + " tilt=" + str(tilt))
pantilthat.pan(pan)
pantilthat.tilt(tilt)
# Sleep for a bit so we're not hammering the HAT with updates
time.sleep(0.01)
def move(self, next_stop=0):
''' Move the pan tilt servos to angles per CAM_STOPS list index
'''
if next_stop >= len(self.CAM_STOPS):
self.cam_pos = 0
# Get the next cam position
self.pan, self.tilt = self.CAM_STOPS[self.cam_pos]
# for pimoroni pantilt servos
# Maximum pan and tilt limits are -90 to + 90
if self.pan > self.PAN_MAX:
self.pan = self.PAN_MAX
if self.pan < -self.PAN_MAX:
self.pan = -self.PAN_MAX
if self.tilt > self.TILT_MAX:
self.tilt = self.TILT_MAX
if self.tilt < -self.TILT_MAX:
self.tilt = -self.TILT_MAX
if self.VERBOSE:
print('pancam - Move to Pos %i/%i at pantilt(%i, %i)' %
(self.cam_pos, len(self.CAM_STOPS), self.pan, self.tilt))
pantilthat.pan(self.pan) # move pan servo
time.sleep(self.PANTILT_DELAY) # give time for servo to move
pantilthat.tilt(self.tilt) # move tilt servo
time.sleep(self.PANTILT_DELAY) # give time for servo to move
self.cam_pos += 1
return self.cam_pos
def rotate():
"""Rotate the camera. Need to pause the camera process otherwise rotating
will trip motion detection due to a vastly different image.
Returns:
str: Response to slack
"""
data = utils.parse_slash_post(request.form)
args = data['text'].split()
if len(args) != 2:
return ("Incorrect input. Please provide as two integers separated by "
" a space. i.e. '0 0'")
try:
pan = int(args[0])
tilt = int(args[1])
except ValueError:
return 'Did not receive integer arguments'
curr_status = utils.redis_get('camera_status')
if curr_status:
utils.redis_set('camera_status', False)
time.sleep(1)
pantilthat.pan(pan)
pantilthat.tilt(tilt)
if curr_status:
utils.redis_set('camera_status', True)
response = 'Successfully panned to {0} and tilted to {1}'.format(pan, tilt)
return response
def run(centerPan, centerTilt, radius, endTime):
print("figureEight running")
currentAngle = -1
#place the lazer on the figure 8
pantilthat.pan(findX(currentAngle, radius, centerPan))
pantilthat.tilt(findY(currentAngle, radius, centerTilt))
time.sleep(0.05)
while time.time() < endTime:
if (currentAngle > 64): #reset the angle to 8 to complete the loop
currentAngle = 0
currentAngle += 1
pan = findX(currentAngle / 10, radius, centerPan)
tilt = findY(currentAngle / 10, radius, centerTilt)
if (boundingPoly.inPanBounds(pan, tilt) == False):
if (pan > boundingPoly.getMaxPan(tilt)):
pan = boundingPoly.getMaxPan(tilt)
if (pan < boundingPoly.getMinPan(tilt)):
pan = boundingPoly.getMinPan(tilt)
if (boundingPoly.inTiltBounds(tilt, pan) == False):
if (tilt > boundingPoly.getMaxTilt(pan)):
tilt = boundingPoly.getMaxTilt(pan)
if (tilt < boundingPoly.getMinTilt(pan)):
tilt = boundingPoly.getMinTilt(pan)
#print ("currentAngle="+ str(currentAngle) + " pan=" + str(pan) + " tilt=" + str(tilt))
pantilthat.pan(pan)
pantilthat.tilt(tilt)
# Sleep for a bit so we're not hammering the HAT with updates
time.sleep(0.02)
def rotate():
"""Rotate the camera. Need to pause the camera process otherwise rotating
will trip motion detection due to a vastly different image.
Returns:
str: Response to slack
"""
data = utils.parse_slash_post(request.form)
args = data['text'].split()
if len(args) != 2:
return ("Incorrect input. Please provide as two integers separated by "
" a space. i.e. '0 0'")
try:
pan = int(args[0])
tilt = int(args[1])
except ValueError:
return 'Did not receive integer arguments'
curr_status = utils.redis_get('camera_status')
if curr_status:
utils.redis_set('camera_status', False)
time.sleep(1)
pantilthat.pan(pan)
pantilthat.tilt(tilt)
if curr_status:
utils.redis_set('camera_status', True)
response = 'Successfully panned to {0} and tilted to {1}'.format(pan, tilt)
return response
def handle_pan_command(self):
"""manage a request to set pan/tilt values"""
match = re.match(r"/pan\?pan=(.*)&tilt=(.*)", self.path)
if match:
try:
pan = float(match.group(1))
tilt = float(match.group(2))
except ValueError:
self.fail()
else:
if pan > 180:
pan -= 360
if pan < -90:
pan = -85
if pan > 90:
pan = 85
print(f"PAN: {pan} {tilt}")
self.send_html(
bytes(
f"<html><body>Pan: {pan}<br>Tilt: {tilt}</body></html>",
encoding="utf-8"))
pantilthat.pan(pan)
pantilthat.tilt(tilt)
else:
self.fail()
def on_after_startup(self):
global pigpioUsed
self._settings.set(["lockState"], "false")
xAutoAngle = self._settings.get_int(["xAutoAngle"])
yAutoAngle = self._settings.get_int(["yAutoAngle"])
libraryUsed = self._settings.get(["chosenOption"])
self._settings.set(["libraryUsed"], libraryUsed)
self._settings.save()
self._logger.info("The libraryUsed is {}".format(libraryUsed))
if libraryUsed == "pigpio":
pigpioUsed = True
if self.pi is None:
self._logger.info("Initializing pigpio")
self.pi = pigpio.pi()
self._logger.info(self.pi)
if not self.pi.connected:
self._logger.info("There was an error initializing pigpio")
return
GPIOX = self._settings.get_int(["GPIOX"])
GPIOY = self._settings.get_int(["GPIOY"])
self.pi.set_servo_pulsewidth(GPIOX, self.angle_to_width(xAutoAngle))
self.pi.set_servo_pulsewidth(GPIOY, self.angle_to_width(yAutoAngle))
"""self._settings.set(["currentX"], xAutoAngle)
self._settings.set(["currentY"], yAutoAngle)
self._settings.save()"""
else:
pigpioUsed = False
pantilthat.pan(self.angle_to_pimoroni(xAutoAngle))
pantilthat.tilt(self.angle_to_pimoroni(xAutoAngle))
"""self._settings.set(["currentX"], xAutoAngle)
def refreshTilt(self):
# self.current_tilt_angle=self.current_tilt_angle+self.offset_tilt
if (self.current_tilt_angle > self.MAX_TILT):
self.current_tilt_angle = self.MAX_TILT
if (self.current_tilt_angle < -self.MAX_TILT):
self.current_tilt_angle = -self.MAX_TILT
pantilthat.tilt(self.current_tilt_angle)
def drawline(panright=0, panleft=0, step=0):
"""Draws a line panning right left, left right offset starts line at circle"""
pantilthat.tilt(TILT)
for pan_deg in range(panright, panleft, step):
pantilthat.pan(pan_deg)
time.sleep(0.10)
#print("Line TILT=%d, PAN=%d" % (TILT, pan_deg))
return
def center_camera(objxy, screencenter, debug=False):
'''
from an (X,Y), it tries to pan/tilt to minimize the distance with the center of the "view"
NOTE: From the viewer's pov: If face is upper left:
-> dX < 0 & dY >0
-> pan++ aims left, tilt-- aims up
Args:
objxy: Tuple (x,y) of the detected object
screencenter: Tuple(x,y) with the center of the screen
Returns:
1 if moved
0 if not
'''
max_angle = 80 # To stay safe and not exeed the max angle of the servo
stepx = MINANGLESTEP # 1 by default #+ abs(int(objxy[0]/100))
stepy = MINANGLESTEP
dX = screencenter[0] - objxy[0]
dY = screencenter[1] - objxy[1]
stepx = int(abs(dX)/10 - IMGTHRESHOLD/10) # Empirical value. 10% of the distance, good enough
stepy = int(abs(dY)/10 - IMGTHRESHOLD/10)
currentPan = pantilthat.get_pan()
currentTilt = pantilthat.get_tilt()
# Computing the next angle, starting on the current
newPan = currentPan
newTilt = currentTilt
if dX < 0 - IMGTHRESHOLD: # and abs(currentPan) + stepx < max_angle:
newPan = currentPan + stepx
newPan = newPan if newPan < max_angle else max_angle
elif dX > 0 + IMGTHRESHOLD: # and abs(currentPan) + stepx < max_angle:
newPan = currentPan - stepx
newPan = newPan if newPan > -max_angle else -max_angle
if dY < 0 - IMGTHRESHOLD: # and abs(currentTilt) + stepy < max_angle:
newTilt = currentTilt + stepy
newTilt = newTilt if newTilt < max_angle else max_angle
elif dY > 0 + IMGTHRESHOLD: # and abs(currentTilt) + stepy < max_angle:
newTilt = currentTilt - stepy
newTilt = newTilt if newTilt > -max_angle else -max_angle
if debug:
print(f"({objxy}) status: pan:{currentPan}, tilt:{currentTilt}; (dX:{dX}, dy:{dY}, step:{stepx},{stepy})({newPan},{newTilt})")
result = 1 if newPan != currentPan or newTilt != currentTilt else 0
pantilthat.pan(newPan)
pantilthat.tilt(newTilt)
return result
def do_GET(self):
if self.path == '/':
self.send_response(301)
self.send_header('Location', '/index.html')
self.end_headers()
elif self.path == '/index.html':
content = PAGE.encode('utf-8')
self.send_response(200)
self.send_header('Content-Type', 'text/html')
self.send_header('Content-Length', len(content))
self.end_headers()
self.wfile.write(content)
elif self.path == '/stream.mjpg':
self.send_response(200)
self.send_header('Age', 0)
self.send_header('Cache-Control', 'no-cache, private')
self.send_header('Pragma', 'no-cache')
self.send_header('Content-Type', 'multipart/x-mixed-replace; boundary=FRAME')
self.end_headers()
try:
while True:
with output.condition:
output.condition.wait()
frame = output.frame
self.wfile.write(b'--FRAME\r\n')
self.send_header('Content-Type', 'image/jpeg')
self.send_header('Content-Length', len(frame))
self.end_headers()
self.wfile.write(frame)
self.wfile.write(b'\r\n')
except Exception as e:
logging.warning(
'Removed streaming client %s: %s',
self.client_address, str(e))
elif self.path == '/up':
#if(tiltangle+5<90):
# tiltangle+=5
pantilthat.tilt(90)
self.send_response(301)
self.send_header('Location', '/index.html')
self.end_headers()
elif self.path == '/down':
# if(tiltangle-5>-90):
# tiltangle-=5
pantilthat.tilt(-90)
self.send_response(301)
self.send_header('Location', '/index.html')
self.end_headers()
else:
self.send_error(404)
self.end_headers()
def drawcircle(panoffset=0, start=0, stop=360, step=2):
"""Draw a circle from cos / sin """
for servo_deg in range(start, stop, step):
x_cart = panoffset + (8 * math.cos(math.radians(servo_deg)))
y_cart = 8 * math.sin(math.radians(servo_deg))
pantilthat.pan(x_cart)
pantilthat.tilt(y_cart + TILT)
time.sleep(DELAY)
#print("Circle TILT=%d, PAN=%d" % (y_cart + TILT, x_cart))
return
def moveRelative(dir, angle):
with VAR.lock:
if dir == 'left' or dir == 'down':
angle = -angle
if dir == 'left' or dir == 'right':
angle += pantilthat.get_pan()
pantilthat.pan(median([-90, angle, 90]))
elif dir == 'up' or dir == 'down':
angle += pantilthat.get_tilt()
pantilthat.tilt(median([-90, angle, 90]))
def _tilt(self, value):
if value == self.curr_y: # already there, don't waste time
return
# print("tilt:", value)
try:
pantilthat.tilt(value)
except Exception as e: # example: value outside [-90,90] is presented.
print(str(e))
self.curr_y = value
time.sleep(DataDict["short_wait"])
def pantilt_test():
while True:
# Get the time in seconds
t = time.time()
# G enerate an angle using a sine wave (-1 to 1) multiplied by 90 (-90 to 90)
a = int(math.sin(t * 2) * 90)
pantilthat.pan(a)
pantilthat.tilt(a)
# Sleep for a bit so we're not hammering the HAT with updates
time.sleep(0.005)
def _tilt_until_value_is_zero(self, value):
_step = 0.5
_dead_zone = 10
if value > 0 + _dead_zone:
self.tilt += _step
if value < 0 - _dead_zone:
self.tilt -= _step
pt.tilt(int(self.tilt))
def look(item):
if item == 'door':
pantilthat.pan(0)
pantilthat.tilt(0)
time.sleep(1)
return "viewing door"
elif item == 'workshop'
pantilthat.pan(90)
pantilthat.tilt(45)
time.sleep(1)
return "viewing workshop"
def initialize():
"""Initialize the security system app
"""
# set redis variables
LOGGER.info('Initializing camera redis variables')
pantilthat.pan(40)
pantilthat.tilt(10)
utils.redis_set('home', False)
utils.redis_set('auto_detect_status', True)
utils.redis_set('camera_status', True)
utils.redis_set('camera_notifications', True)
LOGGER.info('Initialization complete')
return "Initialization completed"
def smooth_tilt(start, to, timeout=0.005):
# print("start tilt",start,"to",to)
count = start
if (start > to):
while count > to:
count = count - 1
pantilthat.tilt(count)
time.sleep(timeout)
else:
while count < to:
count = count + 1
pantilthat.tilt(count)
time.sleep(timeout)
def set_servos(pan, tilt):
signal.signal(signal.SIGINT, signal_handler)
while True:
pan_angle = -1 * pan.value
tilt_angle = tilt.value
# if the pan angle is within the range, pan
if in_range(pan_angle, SERVO_MIN, SERVO_MAX):
pth.pan(pan_angle)
if in_range(tilt_angle, SERVO_MIN, SERVO_MAX):
pth.tilt(tilt_angle)
def camera_test():
pantilthat.pan(0)
pantilthat.tilt(0)
camera = PiCamera()
logging.info('Starting Raspberry Pi Camera')
camera.start_preview()
try:
while True:
continue
except KeyboardInterrupt:
logging.info('Stopping Raspberry Pi Camera')
camera.stop_preview()
def initialize():
"""Initialize the security system app
"""
# set redis variables
LOGGER.info('Initializing camera redis variables')
pantilthat.pan(40)
pantilthat.tilt(10)
utils.redis_set('home', False)
utils.redis_set('auto_detect_status', True)
utils.redis_set('camera_status', True)
utils.redis_set('camera_notifications', True)
LOGGER.info('Initialization complete')
return "Initialization completed"
def center_camera():
"""center_camera() surprisingly centers the camera.
Arguements: none
Returns: nothing
"""
# Interesting. The pan/tilt mast is looking down too far at 90,
# and the pan is not centered at zero.
# Fix it with device-specific values.
pantilthat.tilt(80)
pantilthat.pan(-12)
def set_servos(pan, tlt):
# signal trap to handle keyboard interrupt
signal.signal(signal.SIGINT, signal_handler)
# loop indefinitely
while True:
# the pan and tilt angles are reversed
panAngle = -1 * pan.value
tiltAngle = -1 * tlt.value
# if the pan angle is within the range, pan
if in_range(panAngle, servoRange[0], servoRange[1]):
pth.pan(panAngle)
# if the tilt angle is within the range, tilt
if in_range(tiltAngle, servoRange[0], servoRange[1]):
pth.tilt(tiltAngle)
def directionPositive(direction, angle):
#Function for setting the new angle
#and for turing on the LED's
if direction == "tilt":
currAngle = pantilthat.get_tilt()
newAngle = servoFormulaPositive(currAngle, angle)
os.system('sudo ./piiotest blink 20 1')
pantilthat.tilt(newAngle)
os.system('sudo ./piiotest blink 20 0')
elif direction == "pan":
currAngle = pantilthat.get_pan()
newAngle = servoFormulaPositive(currAngle, angle)
os.system('sudo ./piiotest writepin 20 1')
pantilthat.pan(newAngle)
os.system('sudo ./piiotest writepin 20 0')
def api(direction, angle):
if angle < 0 or angle > 180:
return "{'error':'out of range'}"
angle -= 90
if direction == 'pan':
pantilthat.pan(angle)
return "{{'pan':{}}}".format(angle)
elif direction == 'tilt':
pantilthat.tilt(angle)
return "{{'tilt':{}}}".format(angle)
return "{'error':'invalid direction'}"
def set_angle(direction, angle):
if angle < 0 or angle > 180:
return '{"error": "out of range"}'
angle -= 90
if direction == 'pan':
pantilthat.pan(angle)
return '{{"pan":{}}}'.format(angle + 90)
elif direction == 'tilt':
pantilthat.tilt(angle)
return '{{"tilt":{}}}'.format(angle + 90)
return '{"error": "invalid direction"}'
def movecam(direction, angle):
if angle < 0 or angle > 180:
return "{'error':'out of range'}"
angle = angle - 90
if (angle < -90 or angle > 90):
return getPosition()
print("PANTILT: Operation:[" + direction + "], Angle:[" + str(angle) + "]")
if direction == 'pan':
pantilthat.pan(angle)
elif direction == 'tilt':
pantilthat.tilt(angle)
return getPosition()
def pantilt_test():
logging.info('Starting Pan-Tilt HAT test!')
logging.info('Pan-Tilt HAT should follow a smooth sine wave')
while True:
# Get the time in seconds
t = time.time()
# G enerate an angle using a sine wave (-1 to 1) multiplied by 90 (-90 to 90)
a = math.sin(t * 2) * 90
# Cast a to int for v0.0.2
a = int(a)
pantilthat.pan(a)
pantilthat.tilt(a)
# Sleep for a bit so we're not hammering the HAT with updates
time.sleep(0.005)
def set_servos(pan, tlt, toCenter):
# signal trap to handle keyboard interrupt
signal.signal(signal.SIGINT, signal_handler)
# loop indefinitely
while True:
# the pan and tilt angles are reversed
# prevPan = panAngle
# prevTilt = tltAngle
panAngle = -1 * pan.value
tltAngle = -1 * tlt.value
if toCenter.value == 1:
panAngle = 0
tltAngle = 0
waitForServo = False
if abs(panAngle - pth.get_pan()) > MIN_ACCEPTABLE_ROTATION:
waitForServo = True
#print("detected suden shift in angle")
if abs(tltAngle - pth.get_tilt()) > MIN_ACCEPTABLE_ROTATION:
waitForServo = True
#print("detected suden shift in angle")
# if the pan angle is within the range, pan
if in_range(panAngle, servoRangePan[0], servoRangePan[1]):
# if abs(prevPan - panAngle) < 60 :
# pth.pan(panAngle)
#else:
# pth.pan(int(panAngle/2))
pth.pan(panAngle)
#print(panAngle)
# if the tilt angle is within the range, tilt
if in_range(tltAngle, servoRangeTilt[0], servoRangeTilt[1]):
#if abs(prevTilt - tltAngle) < 60 :
# pth.tilt(tltAngle)
#else:
# pth.tilt(int(tltAngle/2))
pth.tilt(tltAngle)
#print(tltAngle)
if waitForServo is True:
time.sleep(0.6)
print("waiting cuz camera stabilization")
time.sleep(0.1)
def point_camera(panval, tiltval):
"""point_camera() uses the pan/tilt mast to point the camera in
a particular azimuth and elevation.
Note: the mapping from the arguement values to actual direction and
elevation angle has not been determined. We will need to do this
experimentally
Arguements:
pan - direction to pan the camera. Positive is left.
tilt - angle to tilt the camera. Negative is up.
Returns: nothing
"""
# Point the camera
pantilthat.pan(panval) # positive is left from camera's POV
pantilthat.tilt(tiltval) # negative is "up"
def _translation_to_angles(self, translation_vector):
x = translation_vector.x
y = translation_vector.y
z = translation_vector.z
print(x, y, z)
if abs(x) > 0.05:
rad = math.atan2(x, z)
pan = math.degrees(rad)
pt.pan(int(pan) * -1)
print('pan:', pan)
if abs(y) > 0.05:
rad = math.atan2(y, z)
tilt = math.degrees(rad)
pt.tilt(int(tilt))
print('tilt', tilt)
#!/usr/bin/env python
import math
import time
import pantilthat
while True:
# Get the time in seconds
t = time.time()
# G enerate an angle using a sine wave (-1 to 1) multiplied by 90 (-90 to 90)
a = math.sin(t * 2) * 90
# Cast a to int for v0.0.2
a = int(a)
pantilthat.pan(a)
pantilthat.tilt(a)
# Two decimal places is quite enough!
print(round(a,2))
# Sleep for a bit so we're not hammering the HAT with updates
time.sleep(0.005)