pi@rpi:~ $ nano servo.py
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import time
import datetime
import sys
import RPi.GPIO as GPIO
backend_mode = True
def initGPIO():
GPIO.setmode(GPIO.BOARD)
GPIO.setup(11, GPIO.OUT)
GPIO.setup(12, GPIO.OUT)
GPIO.cleanup()
GPIO.setmode(GPIO.BOARD)
GPIO.setup(11, GPIO.OUT)
GPIO.setup(12, GPIO.OUT)
class servoSG90():
def __init__(self, GPIO_Pin=12, PWM_Freq=50, pwm0=5, pwm100=10):
self.dc0 = pwm0
self.dc100 = pwm100
self.pwm_now = int( (7. - pwm0)/(pwm100-pwm0) )
self.deg_now = 90
self.pwm_gpio = GPIO_Pin
self.pwm_freq = PWM_Freq
self.servo = GPIO.PWM(GPIO_Pin,50)
self.servo.start( (self.dc0+self.dc100)/2 )
#self.setRawPWM( self.pwm_now )
self.setPWM( self.pwm_now )
time.sleep(1)
def setPWM(self, pwm): ## Range 0 - 100
if pwm > self.pwm_now:
pwm_step = 1
elif pwm < self.pwm_now:
pwm_step = -1
else:
return
for pwm_new in range(self.pwm_now, int(pwm), pwm_step):
dc_new = round( ( self.dc100 - self.dc0 )/100.0*pwm_new + self.dc0 ,1)
if dc_new > self.dc100:
dc_new = self.dc100
elif dc_new < self.dc0:
dc_new = self.dc0
self.servo.ChangeDutyCycle( dc_new )
print(" --- Change PWM Steping... %d - %.1f --- %.1f - %.1f"%(pwm_new, dc_new, self.dc0, self.dc100) )
time.sleep(.1)
self.pwm_now = int(pwm_new)
def setDegree(self, deg):
self.deg_now = 90
def setRawPWM(self, pwm):
#self.servo = GPIO.PWM(self.pwm_gpio, self.pwm_freq)
#self.servo.start( pwm )
self.servo.ChangeDutyCycle( pwm )
time.sleep(0.2)
def stop(self):
self.servo.ChangeDutyCycle( 7 )
time.sleep(1)
self.servo.stop()
def main():
initGPIO()
servo1 = servoSG90(GPIO_Pin=12, pwm0=3, pwm100=11.5)
servo2 = servoSG90(GPIO_Pin=11, pwm0=5, pwm100=10)
with open("sg90.cfg", "w") as sg:
sg.write("50,50")
try:
while True:
with open("sg90.cfg", "r") as sg:
raw = sg.readline()
if 'stop' in raw.lower():
break
(x,y) = eval(raw)
servo1.setPWM(x)
servo2.setPWM(y)
print("SG90 : %s --- %d - %d"%(raw, x, y) )
time.sleep(2)
except KeyboardInterrupt:
servo1.stop()
servo2.stop()
if __name__ == "__main__":
main()
GPIO.cleanup()
sys.exit()
pi@rpi:~ $ nano face_follow.py
#!/usr/bin/env python
# import the necessary packages
from picamera.array import PiRGBArray
from picamera import PiCamera
import time
import datetime
import cv2
import sys
PiCamResX = 640
PiCamResY = 480
faceXML = "haarcascade_frontalface_alt.xml"
faceCascade = cv2.CascadeClassifier(faceXML)
eyeXML = "haarcascade_eye_tree_eyeglasses.xml"
eyeCascade = cv2.CascadeClassifier(eyeXML)
def initPiCamera():
# initialize the camera and grab a reference to the raw camera capture
camera = PiCamera()
camera.resolution = (PiCamResX, PiCamResY)
camera.framerate = 32
camera.hflip = False
camera.vflip = False # True
rawCapture = PiRGBArray(camera, size=(PiCamResX, PiCamResY))
# allow the camera to warmup
time.sleep(1)
return(camera, rawCapture)
def detectFace(gray):
# Define Face Detect Scale
faces = faceCascade.detectMultiScale(
gray,
scaleFactor=1.1,
minNeighbors=5,
minSize=(30, 30),
flags=cv2.CASCADE_SCALE_IMAGE
)
return(faces)
def drawCross(img, x, y):
cv2.circle(img, (x, y), 10, (0, 255, 0), 1)
cv2.line(img, (x-20, y), (x+20, y), (0, 255, 0), 1)
cv2.line(img, (x, y-20), (x, y+20), (0, 255, 0), 1)
class servoPID():
def __init__(self, x=50, y=50):
self.pos_x_tar_hi = PiCamResX/2 + 30
self.pos_x_tar_lo = PiCamResX/2 - 30
self.pos_x_err0, self.pos_x_err1, self.pos_x_err2 = 0,0,0
self.pos_y_tar_hi = PiCamResY/2 + 25
self.pos_y_tar_lo = PiCamResY/2 - 25
self.pos_y_err0, self.pos_y_err1, self.pos_y_err2 = 0,0,0
self.setPWM(x,y)
def setPWM(self, x,y):
with open("sg90.cfg", "w") as sg:
sg.write("%d,%d"%(x,y))
#print("New Location : %d, %d"%(x,y) )
time.sleep(3)
def calcPWM(self, pos_x, pos_y):
with open("sg90.cfg", "r") as sg:
raw = sg.readline()
(x0,y0) = eval(raw)
self.pos_x_err2 = self.pos_x_err1
self.pos_x_err1 = self.pos_x_err0
if pos_x > self.pos_x_tar_hi:
self.pos_x_err0 = pos_x - self.pos_x_tar_hi
elif pos_x < self.pos_x_tar_lo:
self.pos_x_err0 = pos_x - self.pos_x_tar_lo
else:
self.pos_x_err0 = 0
pwm_x_err = 0.03*(self.pos_x_err0-self.pos_x_err1) + 0.05*self.pos_x_err0 + 0.01*(self.pos_x_err0-2*self.pos_x_err1+self.pos_x_err2)
x1_tmp = x0 - pwm_x_err
x1 = self.checkRange(x1_tmp)
self.pos_y_err2 = self.pos_y_err1
self.pos_y_err1 = self.pos_y_err0
if pos_y > self.pos_y_tar_hi:
self.pos_y_err0 = pos_y - self.pos_y_tar_hi
elif pos_y < self.pos_y_tar_lo:
self.pos_y_err0 = pos_y - self.pos_y_tar_lo
else:
self.pos_y_err0 = 0
pwm_y_err = 0.02*(self.pos_y_err0-self.pos_y_err1) + 0.03*self.pos_y_err0 + 0.01*(self.pos_y_err0-2*self.pos_y_err1+self.pos_y_err2)
y1_tmp = y0 + pwm_y_err
y1 = self.checkRange(y1_tmp)
#print("New Location : %d --- %d - 320 --- %d --- %.1f - %d"%(x1, pos_x, x1_tmp, self.pos_x_err0, pwm_x_err) )
#print(" New Location : %d --- %d - 320 --- %d --- %.1f - %d"%(y1, pos_y, y1_tmp, self.pos_y_err0, pwm_y_err) )
self.setPWM(x1, y1)
def checkRange(self, pwm):
if pwm > 90:
return( 90 )
elif pwm < 10:
return( 10 )
else:
return( int(pwm) )
def stop(self):
with open("sg90.cfg", "w") as sg:
sg.write("50,50")
def main():
servo = servoPID(50,10)
(camera, rawCapture) = initPiCamera()
# capture frames from the camera
for frame in camera.capture_continuous(rawCapture, format="bgr", use_video_port=True):
# grab the raw NumPy array representing the image, then initialize the timestamp
# and occupied/unoccupied text
image = frame.array
gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
PiCamResX/2 + 30
cv2.rectangle(image, (PiCamResX/2-30, 210), (PiCamResX/2+30, 270), (0, 255, 255), 1)
faces = detectFace(gray)
# Draw a rectangle around the faces and Eyes
area, pos_x, pos_y = 0, 0, 0
for (x, y, w, h) in faces:
cv2.rectangle(image, (x, y), (x+w, y+h), (0, 255, 0), 2)
area_tmp = w*h
if area_tmp > area:
area = area_tmp
pos_x = x + w/2
pos_y = y + h/2
cv2.putText(image, "Face Pos : (%d, %d)"%(pos_x, pos_y), (10,50), cv2.FONT_HERSHEY_TRIPLEX, 1, (255,255,0), 1)
drawCross(image, pos_x, pos_y)
# show the frame
cv2.imshow("Detect Face - Press 'q' to stop", image)
key = cv2.waitKey(1) & 0xFF
if pos_x > 10 and pos_y > 10:
servo.calcPWM( pos_x, pos_y )
# clear the stream in preparation for the next frame
rawCapture.truncate(0)
# if the `q` key was pressed, break from the loop
if key == ord("q"):
rawCapture.truncate(0)
cv2.destroyAllWindows()
print("\n\n --- Faces --- %s \n%s"%(type(faces), faces) )
servo.stop()
break
# Show the results
cv2.imshow("Detect Face - Done", image)
cv2.waitKey(0)
cv2.imwrite("face_detect_%s.jpg"%datetime.datetime.now().strftime("%Y%m%d-%H%M%S") , image, [int(cv2.IMWRITE_JPEG_QUALITY), 80])
if __name__ == "__main__":
main()
sys.exit()