def followBall(neckDegree, headDegree, debug=False):
#gradi limite del collo
MAX_NECK_DEGREE = 40
MIN_NECK_DEGREE = -40
MAX_HEAD_DEGREE = 40
MIN_HEAD_DEGREE = -40
ballCenter = None
#pixel di soglia e step in base alla distanza dal centro
PIXEL_THRESHOLD = 30
#L_DEGREE_STEP = 4
#M_DEGREE_STEP = 6
#H_DEGREE_STEP = 8
L_DEGREE_STEP = 2
M_DEGREE_STEP = 4
H_DEGREE_STEP = 6
#cerca la palla all'interno del frame, e se la trova..
detected = detectBall(debug=debug)
if detected is not None:
ballCenter, _ = detected
#ottiene le differenze del centro rispetto alle coordinate della camera
w_diff = ballCenter[0] - cameraW // 2
h_diff = ballCenter[1] - cameraH // 2
#decide di quanto muoversi in base alla distanza dal centro
wDegreeStep = L_DEGREE_STEP
if (abs(w_diff) > (cameraW // 8) * 3): wDegreeStep = H_DEGREE_STEP
elif (abs(w_diff) > cameraW // 4): wDegreeStep = M_DEGREE_STEP
hDegreeStep = L_DEGREE_STEP
if (abs(h_diff) > (cameraH // 8) * 3): hDegreeStep = H_DEGREE_STEP
elif (abs(h_diff) > cameraH // 4): hDegreeStep = M_DEGREE_STEP
#decide in quale direzione andare
if (w_diff > PIXEL_THRESHOLD): neckDegree += wDegreeStep
elif (w_diff < -PIXEL_THRESHOLD): neckDegree -= wDegreeStep
if (h_diff > PIXEL_THRESHOLD): headDegree += hDegreeStep
elif (h_diff < -PIXEL_THRESHOLD): headDegree -= hDegreeStep
#limita entro un range i movimenti
neckDegree = max(min(neckDegree, MAX_NECK_DEGREE), MIN_NECK_DEGREE)
headDegree = max(min(headDegree, MAX_HEAD_DEGREE), MIN_HEAD_DEGREE)
#muove la testa
jd.moveJoint(jd.NECK, neckDegree)
jd.moveJoint(jd.HEAD, headDegree)
#torna le nuove coordinate
return ballCenter, neckDegree, headDegree
def mapServoPosition(x, y):
global panAngle
global tiltAngle
w_diff = x - 500 // 2
h_diff = y - 250 // 2
PIXEL_THRESHOLD = 20
if (w_diff > PIXEL_THRESHOLD): panAngle += 2
elif (w_diff < -PIXEL_THRESHOLD): panAngle -= 2
if (h_diff > PIXEL_THRESHOLD): tiltAngle += 2
elif (h_diff < -PIXEL_THRESHOLD): tiltAngle -= 2
jd.moveJoint(jd.NECK, panAngle)
jd.moveJoint(jd.HEAD, tiltAngle)
def followFace(neckDegree, headDegree, debug=False):
MAX_NECK_DEGREE = 35
MIN_NECK_DEGREE = -35
MAX_HEAD_DEGREE = 35
MIN_HEAD_DEGREE = -35
DEGREE_STEP_SIZE = 2
PIXEL_THRESHOLD = 10
faceRect = None
#cerca la faccia all'interno del frame, e se la trova..
detected = detectFace()
if detected is not None:
faceRect = detected[-1:]
faceCenterX = faceRect[0] + faceRect[2] // 2
faceCenterY = faceRect[1] + faceRect[3] // 2
#ottiene differenze con le coordinate della camera
w_diff = faceCenterX - cameraW // 2
h_diff = faceCenterY - cameraH // 2
#decide in quale direzione andare
if (w_diff > PIXEL_THRESHOLD):
neckDegree += DEGREE_STEP_SIZE
print("- collo a destra %s" % (w_diff))
if (w_diff < -PIXEL_THRESHOLD):
neckDegree -= DEGREE_STEP_SIZE
print("- collo a sinstra %s" % (w_diff))
if (h_diff > PIXEL_THRESHOLD):
headDegree += DEGREE_STEP_SIZE
print("- testa su %s" % (h_diff))
if (h_diff < -PIXEL_THRESHOLD):
headDegree -= DEGREE_STEP_SIZE
print("- testa giu %s" % (h_diff))
#limita entro un range i movimenti
neckDegree = max(min(neckDegree, MAX_NECK_DEGREE), MIN_NECK_DEGREE)
headDegree = max(min(headDegree, MAX_HEAD_DEGREE), MIN_HEAD_DEGREE)
#muove la testa
jd.moveJoint(jd.NECK, neckDegree)
jd.moveJoint(jd.HEAD, headDegree)
#torna le nuove coordinate
return faceRect, neckDegree, headDegree
def exercise1(joint):
jd.moveJoint(joint, 0)
time.sleep(0.3)
for degree in [30, 0, 30, 0, 30, 0]:
jd.moveJoint(joint, degree)
time.sleep(0.2)
time.sleep(0.5)
for degree in [-30, 0, -30, 0, -30, 0]:
jd.moveJoint(joint, degree)
time.sleep(0.2)
time.sleep(0.5)
def happy():
#si abbassa in avanti e guarda in su
jd.moveJoint(jd.RIGHT_BACK_ARM, 60)
jd.moveJoint(jd.LEFT_BACK_ARM, 60)
jd.moveJoint(jd.RIGHT_FRONT_ARM, 0)
jd.moveJoint(jd.LEFT_FRONT_ARM, 0)
jd.moveJoint(jd.HEAD, 10)
time.sleep(0.4)
#nuove il dietro come un cane
for degree in [30, -30, 30, -30, 30, -30, 30, -30, 0]:
jd.moveJoint(jd.RIGHT_BACK_SHOULDER, -degree)
jd.moveJoint(jd.LEFT_BACK_SHOULDER, degree)
time.sleep(0.2)
for degree in [30, -30, 30, -30, 30, -30, 30, -30, 0]:
jd.moveJoint(jd.RIGHT_BACK_SHOULDER, -degree)
jd.moveJoint(jd.LEFT_BACK_SHOULDER, degree)
time.sleep(0.2)
#standup iniziale
if(args.command == "standup"):
arm_zero_pos = 50
jd.zero(arm_zero_pos)
time.sleep(0.5)
#effettua leggeri movimenti random
for _ in range(50):
mov = random.choice([0, 15, -15])
jd.moveJoint(jd.RIGHT_FRONT_ARM, arm_zero_pos-mov)
jd.moveJoint(jd.LEFT_FRONT_ARM, arm_zero_pos+mov)
jd.moveJoint(jd.RIGHT_BACK_ARM, arm_zero_pos-mov)
jd.moveJoint(jd.LEFT_BACK_ARM, arm_zero_pos+mov)
jd.moveJoint(jd.HEAD, random.choice([0, 3, -3, 0, 2, -2]))
jd.moveJoint(jd.NECK, random.choice([0, 4, -4, 0, 2, -2]))
time.sleep(1)
time.sleep(4)
jd.relax()
#prova i vari passi: avanti, indietro, ruota a destra, ruota a sinistra
if(args.command == "steps"):
#va avanti
jd.stepForward(2)
# initialize the video stream and allow the camera sensor to warmup
print("[INFO] waiting for camera to warmup...")
vs = VideoStream(0).start()
time.sleep(2.0)
# Initialize angle servos at 90-90 position
global panAngle
global tiltAngle
#panAngle = 90
#tiltAngle =90
panAngle = 0
tiltAngle = 0
# positioning Pan/Tilt servos at initial position
jd.moveJoint(jd.NECK, panAngle)
jd.moveJoint(jd.HEAD, tiltAngle)
# loop over the frames from the video stream
while True:
# grab the next frame from the video stream, Invert 180o, resize the
# frame, and convert it to the HSV color space
frame = vs.read()
frame = imutils.resize(frame, width=500)
frame = imutils.rotate(frame, angle=180)
hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
# construct a mask for the object color, then perform
# a series of dilations and erosions to remove any small
# blobs left in the mask
#mask = cv2.inRange(hsv, (24, 100, 100), (44, 255, 255))
for degree in [-30, 0, -30, 0, -30, 0]:
jd.moveJoint(joint, degree)
time.sleep(0.2)
time.sleep(0.5)
#posizione iniziale
if (args.command == "standup"):
jd.zero(50)
time.sleep(4)
jd.relax()
if (args.command == "standup2"):
jd.zero(50)
time.sleep(4)
jd.moveJoint(jd.RIGHT_BACK_ARM, 0)
jd.moveJoint(jd.LEFT_BACK_ARM, -20)
time.sleep(1)
for degree_arm, degree_schoulder in zip([-40, -40, 40, 40],
[0, 40, 40, 0]):
jd.moveJoint(jd.RIGHT_FRONT_ARM, degree_arm)
time.sleep(0.2)
jd.moveJoint(jd.RIGHT_FRONT_SHOULDER, degree_schoulder)
time.sleep(0.2)
time.sleep(4)
jd.zero()
time.sleep(0.2)
jd.relax()
'''
if(args.command.startswith("move")):
movements = args.command.split(":")[1]