class Servo:
def __init__(self, pin, inital_angle=0, reverse=False):
self.servo = None
self.current_angle = inital_angle
angle = self.current_angle - 90
if reverse:
self.servo = AngularServo(pin, angle, min_angle=90, max_angle=-90)
else:
self.servo = AngularServo(pin, angle)
def __set_angle(self, value):
if value is None:
angle = None
else:
self.current_angle = value
angle = self.current_angle - 90
self.servo.angle = angle
def __get_angle(self):
return self.current_angle
angle = property(__get_angle, __set_angle)
def close(self):
self.servo.close()
class actuatorController:
def __init__(self):
self._TRUEZERO = 5
self.throttleScalingFactor = 0.25
IO.setwarnings(False)
IO.setmode(IO.BCM)
IO.setup(12, IO.OUT)
IO.setup(23, IO.OUT, initial=1)
self._p = IO.PWM(12, 500)
self._p.start(0)
self._servo = AngularServo(13, min_angle=-90, max_angle=90)
self._servo.angle = self._TRUEZERO
def actuate(self, direction, throttle, angle, brake):
if not brake:
self._p.ChangeDutyCycle(throttle * self.throttleScalingFactor)
else:
self.throttle = 0
if angle < 0:
self._servo.angle = (angle / 90 * 95) + self._TRUEZERO
elif angle > 0:
self._servo.angle = (angle / 90 * 85) + self._TRUEZERO
else:
self._servo.angle = self._TRUEZERO
IO.output(23, direction)
def stopAllMotors(self):
self._p.stop()
self.throttle = 0
self.brake = True
def releaseBrake(self):
self.brake = False
def setDirectionReverse(self):
self.direction = 0
#only call this on exit
def exit(self):
self._p.stop()
self._servo.close()
IO.cleanup()
self.running = False
# the timer is expired now, rotate servo back and forth
# until timer is cancelled
while self.timer_token:
self._set_servo_to_angle(175, timeout=1)
my_pwm.start(100)
self._set_servo_to_angle(5, timeout=1)
my_pwm.start(0)
# the timer was cancelled, reset the servo back to initial state
self._set_servo_to_angle(0, timeout=1)
my_pwm.start(0)
def _set_servo_to_angle(self, angle_in_degrees, timeout):
"""
Sets the servo to the specified angle. Keep this between 0 and 180
"""
# set the angle of the Servo (min = -90, max = 90)
SERVO.angle = 90 - float(angle_in_degrees)
logger.debug('Setting servo to: ' + str(angle_in_degrees))
time.sleep(timeout)
SERVO.detach()
if __name__ == '__main__':
try:
TimerGadget().main()
finally:
logger.debug('Cleaning up GPIO')
SERVO.close()
def worker(conn, increasepos, increaseinc):
pid = os.getpid()
conn.send(pid)
global deltaAngle
global HdeltaAngle
global VglobalAngle
global HglobalAngle
global minangle
global s
global s2
# move vertical angle by this much
deltaAngle = -int(increaseinc)
# move vertical angle by this much
HdeltaAngle = int(increasepos)
while True:
# recv the command fromt the conctroller
command = conn.recv()
if command == 'Start':
print('\ndeltaAngle =' + str(deltaAngle))
print('HdeltaAngle = ' + str(HdeltaAngle))
try:
f = open("Position.txt", 'r+')
coord = f.read().split(",")
f.close()
s = AngularServo(17, initial_angle=int(coord[1]), min_angle=-60, max_angle=60)
sleep(2)
s2 = AngularServo(21, initial_angle=int(coord[0]), min_angle=-60, max_angle=60)
sleep(2)
except FileNotFoundError:
print("File not found. Setting angles to 0 in 5 seconds...")
sleep(5)
s = AngularServo(17, min_angle=-60, max_angle=60)
sleep(2)
s2 = AngularServo(21, min_angle=-60, max_angle=60)
sleep(2)
write_to(0,0)
sleep(2)
print("ANTENNA INITIALIZED")
print("Waiting 5 seconds before continuing...")
sleep(5)
while s.angle != 10:
s.angle += 10
sleep(2)
write_to(HglobalAngle, VglobalAngle)
while s2.angle != -60:
s2.angle -= 10
sleep(2)
write_to(HglobalAngle, VglobalAngle)
# sends controller that it has finished moving
conn.send([HglobalAngle,VglobalAngle])# checks to see if the next command is to move
write_to(HglobalAngle, VglobalAngle)
elif command == 'Next':
sleep(1)
# if the angle is greater than 50 it'll set the angle to 50 instead of going over
if VglobalAngle == -20:
if HglobalAngle + HdeltaAngle >= 60:
if VglobalAngle == -20 and HglobalAngle == 60:
conn.send("done")
while True:
if conn.recv() == 'movetostrongest':
conn.send('ready')
strongest = conn.recv()
print("moving to strongest signal at: " + str(strongest[0]) +" " + str(strongest[1]))
sleep(3)
moveto(strongest[0], strongest[1])
write_to(strongest[0], strongest[1])
sleep(3)
break
conn.send("Done")
HglobalAngle = 60
s2.angle = 60
sleep(2)
VglobalAngle = minangle
moveto(HglobalAngle, VglobalAngle)
sleep(2)
conn.send([HglobalAngle, VglobalAngle])
write_to(HglobalAngle, VglobalAngle)
sleep(1)
else:
moveHorizontalServo() # increases the HglobalAngle
VglobalAngle = minangle
moveto(HglobalAngle,VglobalAngle)
sleep(2)
conn.send([HglobalAngle, VglobalAngle])
write_to(HglobalAngle, VglobalAngle)
sleep(1)
if VglobalAngle + deltaAngle <= -20:
VglobalAngle = -20
s.angle = -20
sleep(2) # for right now they are set to 1
conn.send([HglobalAngle, VglobalAngle])
write_to(HglobalAngle, VglobalAngle)
sleep(1)
# increments the vertical servo if it won't go over 50
else:
incrementVerticalServo()
sleep(3)
conn.send([HglobalAngle, VglobalAngle])
write_to(HglobalAngle, VglobalAngle)
sleep(3)
elif command == 'moveto':
print("Moving Antenna vertical angle to:", str(increaseinc))
print("Moving Antenna horizontal angle to:", str(increasepos))
try:
f = open("Position.txt", 'r+')
coord = f.read().split(",")
f.close()
s = AngularServo(17, initial_angle=int(coord[1]), min_angle=-60, max_angle=60)
sleep(2)
s2 = AngularServo(21, initial_angle=int(coord[0]), min_angle=-60, max_angle=60)
sleep(2)
except FileNotFoundError:
print("File not found. Setting angles to 0 in 5 seconds...")
sleep(5)
s = AngularServo(17, min_angle=-60, max_angle=60)
sleep(2)
s2 = AngularServo(21, min_angle=-60, max_angle=60)
sleep(2)
write_to(0,0)
sleep(2)
moveto(increasepos, increaseinc)
write_to(increasepos, increaseinc)
sleep(3)
s.close()
sleep(3)
s2.close()
conn.send("close")
exit(0)
cv2.circle(img_frame, (cx, cy), CIRCLE_SIZE, green,
LINE_THICKNESS)
if WINDOW_BIGGER > 1: # Note setting a bigger window will slow the FPS
img_frame = cv2.resize(img_frame, (big_w, big_h))
cv2.imshow('Track q quits', img_frame)
# Close Window if q pressed while movement status window selected
if cv2.waitKey(1) & 0xFF == ord('q'):
vs.stop()
cv2.destroyAllWindows()
print("face_track - End Motion Tracking")
still_scanning = False
#-----------------------------------------------------------------------------------------------
if __name__ == '__main__':
try:
face_track()
except KeyboardInterrupt:
print("")
print("User Pressed Keyboard ctrl-c")
finally:
print("Closing gpiozero pan_pin=%i and tilt_pin=%i" %
(pan_pin, tilt_pin))
pan.close()
tilt.close()
print("")
print("%s %s Exiting Program" % (progName, progVer))
class AUDIO:
def __init__(self):
self.p = pyaudio.PyAudio()
self.jaw = AngularServo(c.JAW_PIN, min_angle=c.MIN_ANGLE,
max_angle=c.MAX_ANGLE, initial_angle=None,
min_pulse_width=c.SERVO_MIN/(1*10**6),
max_pulse_width=c.SERVO_MAX/(1*10**6))
self.bp = BPFilter()
def stop(self):
self.streamAlias.stop_stream()
self.jaw.close()
def play_audio(self, filename=None):
# Used for both threshold (Scary Terry style) and multi-level (jawduino style)
def get_avg(data, channels):
"""Gets and returns the average volume for the frame (chunk).
for stereo channels, only looks at the right channel (channel 1)"""
levels = abs(np.frombuffer(data, dtype='<i2'))
# Apply bandpass filter if STYLE=2
if c.STYLE == 2:
levels = self.bp.filter_data(levels)
levels = np.absolute(levels)
if channels == 1:
avg_volume = sum(levels)//len(levels)
elif channels == 2:
rightLevels = levels[1::2]
avg_volume = sum(rightLevels)//len(rightLevels)
return(avg_volume)
def get_target(data, channels):
volume = get_avg(data, channels)
jawStep = (self.jaw.max_angle - self.jaw.min_angle) / 3
if c.STYLE == 0: # Scary Terry style single threshold
if volume > c.THRESHOLD:
jawTarget = self.jaw.min_angle
else:
jawTarget = self.jaw.max_angle
elif c.STYLE == 1: # Jawduino style multi-level or Wee Talker bandpss multi-level
if volume > c.LEVEL3:
jawTarget = self.jaw.min_angle
elif volume > c.LEVEL2:
jawTarget = self.jaw.min_angle + jawStep
elif volume > c.LEVEL1:
jawTarget = self.jaw.min_angle + 2 * jawStep
else:
jawTarget = self.jaw.max_angle
else: # Jawduino style multi-level or Wee Talker bandpss multi-level
if volume > c.FIlTERED_LEVEL3:
jawTarget = self.jaw.min_angle
elif volume > c.FIlTERED_LEVEL2:
jawTarget = self.jaw.min_angle + jawStep
elif volume > c.FIlTERED_LEVEL1:
jawTarget = self.jaw.min_angle + 2 * jawStep
else:
jawTarget = self.jaw.max_angle
return jawTarget
def overwrite(data, channels):
""" overwrites left channel onto right channel for playback"""
if channels != 2:
raise ValueError("channels must equal 2")
levels = np.frombuffer(data, dtype='<i2')
levels[1::2] = levels[::2]
data = levels.tolist()
return data
def filesCallback(in_data, frame_count, time_info, status):
data = wf.readframes(frame_count)
channels = wf.getnchannels()
jawTarget = get_target(data, channels)
self.jaw.angle = jawTarget
# If only want left channel of input, duplicate left channel on right
if (channels == 2) and (c.OUTPUT_CHANNELS == 'LEFT'):
data = overwrite(data, channels)
return (data, pyaudio.paContinue)
def micCallback(in_data, frame_count, time_info, status):
channels = 1 # Microphone input is always monaural
jawTarget = get_target(in_data, channels)
self.jaw.angle = jawTarget
# If only want left channel of input, duplicate left channel on right
if (channels == 2) and (c.OUTPUT_CHANNELS == 'LEFT'):
in_data = overwrite(in_data, channels)
return (in_data, pyaudio.paContinue)
def normalEnd():
self.streamAlias.stop_stream()
self.streamAlias.close()
if c.SOURCE == 'FILES':
wf.close()
self.jaw.angle = None
def cleanup():
normalEnd()
self.p.terminate()
self.jaw.close()
try:
atexit.register(cleanup)
# open stream using callback
#Playing from wave file
if c.SOURCE == 'FILES':
wf = wave.open(filename, 'rb')
file_sw = wf.getsampwidth()
stream = self.p.open(format=self.p.get_format_from_width(file_sw),
channels=wf.getnchannels(),
rate=wf.getframerate(),
output=True,
stream_callback=filesCallback)
self.streamAlias = stream
while stream.is_active():
time.sleep(0.1)
# Playing from microphone
elif c.SOURCE == 'MICROPHONE':
stream = self.p.open(format=pyaudio.paInt16, channels=1,
rate=48000, frames_per_buffer=1024,
input=True, output=True,
stream_callback=micCallback)
self.streamAlias = stream
if c.PROP_TRIGGER != 'START':
time.sleep(c.MIC_TIME)
stream.close()
else:
while stream.is_active():
time.sleep(1.)
normalEnd()
except (KeyboardInterrupt, SystemExit):
cleanup()
import sys
from gpiozero import AngularServo
from time import sleep
# Just for demonstration, lots of delay , Not suitable for stable PWM
stress_level = float(sys.argv[1])
if stress_level > 100:
stress_level = 100
angle = stress_level * 110 / 100 - 20
servo_left = AngularServo(18,
initial_angle=angle,
min_angle=90,
max_angle=-90,
min_pulse_width=1 / 1000,
max_pulse_width=2.5 / 1000)
servo_right = AngularServo(13,
initial_angle=angle,
min_angle=-90,
max_angle=90,
min_pulse_width=1 / 1000,
max_pulse_width=2.5 / 1000)
sleep(3)
servo_left.close()
servo_right.close()
class Zoltar(object):
def __init__(self, comm_client):
print('Finally, a Zoltar!')
self.is_moving = False
self.left_eye = LED(LED1_GPIO)
self.right_eye = LED(LED2_GPIO)
self.button = Button(BUTTON_GPIO)
self.button.when_pressed = self.stop_moving
self.communications = comm_client
def begin_moving(self):
self.left_eye.on()
self.right_eye.on()
self.servo = AngularServo(SERVO_GPIO,
min_angle=SERVO_MIN_ANGLE,
max_angle=SERVO_MAX_ANGLE,
initial_angle=SERVO_INITIAL_ANGLE)
while self.is_moving:
if self.communications.available():
(origin, message) = self.communications.recv()
if message == 'COIN':
break
for x in reversed(range(0, 179)):
self.servo.angle = x
sleep(0.01)
for x in range(0, 179):
self.servo.angle = x
if self.servo.angle == 0:
sleep(0.5)
else:
sleep(0.01)
self.finale()
def eyes_off(self):
self.right_eye.off()
self.left_eye.off()
def eyes_on(self):
self.left_eye.on()
self.right_eye.on()
def finale(self):
self.flashing_eyes()
self.print_fortune()
self.cleanup_gpio()
def print_fortune(self):
zoltar_dir = os.getenv('ZOLTAR_DIR')
subprocess.Popen(['python', 'zoltar_print_fortune.py'], cwd=zoltar_dir)
def flashing_eyes(self):
self.eyes_off()
sleep(1)
self.eyes_on()
sleep(1)
self.eyes_off()
sleep(1)
self.eyes_on()
sleep(1)
self.eyes_off()
def stop_moving(self):
print('Button detected')
self.is_moving = False
def cleanup_gpio(self):
self.left_eye.close()
self.right_eye.close()
self.servo.close()
self.button.close()
