def Run():
global Socket
#api.Walk(True)
print("Running...")
#packet = Socket.recv(8)
print("Packet recieved")
#command = struct.unpack('B', packet[1])[0]
command = 0
#move foward
print(command)
if (command == 6):
api.WalkMove(0)
api.Walk(True)
api.WalkMove(20)
#move back
elif (command == 7):
print("supposed to move backwards")
#move right
elif (command == 8):
api.WalkMove(0)
api.WalkTurn(20)
#move left
elif (command == 9):
api.WalkMove(0)
api.WalkTurn(-20)
elif (command == 5):
api.WalkMove(0)
elif (command == 0):
api.SetMotorValue(19, 400)
# api.SetMotorValue(20,500)
print(api.GetMotorValue(19))
print(api.GetMotorValue(20))
api.ServoShutdown()
def Run(X, Y, Distance):
global Walk, sit, HeadMoved
#api.Walk(True)
print("Running...")
command = 0
z = 5
# declaring servos limmits
#headRightLim=667
#headLeftLim=364
#headUpLim=612
#headDownLim=433
headRightLim = 600
headLeftLim = 420
headUpLim = 612
headDownLim = 433
pan = api.GetMotorValue(19)
tilt = api.GetMotorValue(20)
# check when to call the walk functions
if ((pan < headRightLim) and (pan > headLeftLim)):
print "head move"
elif (pan > headRightLim):
# call walk to the left till you reach pan = 512 (need while loop)
# while (pan != 512):
print "walk to the left"
elif (pan < headLeftLim):
# call walk to the right till you reach pan = 512 (need while loop)
# while (pan != 512):
print "walk to the right"
if HeadMoved == 0:
if (X > 50) and (667 > pan):
for i in range(1, z):
api.SetMotorValue(19, pan + 7)
elif (X < -50) and (364 < pan):
for i in range(1, abs(z)):
api.SetMotorValue(19, pan - 7)
if (Y > 50) and (612 > tilt):
for i in range(1, z):
api.SetMotorValue(20, tilt + 7)
elif (Y < -50) and (433 < tilt):
for i in range(1, abs(z)):
api.SetMotorValue(20, tilt - 7)
print "distance", Distance
# if (Distance < 150) and (Walk == False):
# api.Walk(True)
# Walk = True
# sit = 0
# HeadMoved = 1
# print('Walk=======>')
# elif(Distance > 150) and (Walk == True) :
#api.Walk(False)
# Walk=False
# if (sit == 0):
# api.Walk(False)
# api.PlayAction(15)
# sit = 1
# print ( 'Sitting+++++++++')
# if (Walk == True):
# if (X>50):
# api.WalkTurn(5)
# print('Trun Right')
# elif (X<-50):
# api.WalkTurn(-5)
# print('Trun Left')
# else :
# api.WalkTurn(0)
# print ('No Trun')
print("pan", pan)
print("tilt", tilt)
rects = detect(gray, cascade)
vis = img.copy()
draw_rects(vis, rects, (0, 255, 0))
if not nested.empty():
for x1, y1, x2, y2 in rects:
roi = gray[y1:y2, x1:x2]
vis_roi = vis[y1:y2, x1:x2]
subrects = detect(roi.copy(), nested)
#draw_rects(vis_roi, subrects, (255, 0, 0))
# print ('time: %.1f ms' % (dt*1000))
# print ('Length: %.1f cm' % (y2-y1))
x_center = (320 - (x2 + x1) / 2)
y_center = (240 - (y2 + y1) / 2)
#Run(x_center,y_center)
Run(x_center, y_center, y2 - y1)
#print (x_center,y_center)
if 0xFF & cv2.waitKey(5) == 27:
break
cv2.destroyAllWindows()
except (KeyboardInterrupt):
print
print("ID 20 :", api.GetMotorValue(20))
api.ServoShutdown()
sys.exit()
# this steps to read servo's value-- add break to the maim while then uncomment the next two lines
#print (api.GetMotorValue(20))
#api.ServoShutdown()
def Run(X,Y,Distance):
global Walk, sit, HeadMoved,i,j
#api.Walk(True)
print("Running...")
command = 0
z=5
# declaring servos limmits
#headRightLim=667
#headLeftLim=364
#headUpLim=612
#headDownLim=433
headRightLim=600
headLeftLim=420
headUpLim=612
headDownLim=433
pan = api.GetMotorValue(19)
tilt= api.GetMotorValue(20)
# check when to call the walk functions
if ((pan<headRightLim) and (pan>headLeftLim)):
print "head move"
elif (pan>headRightLim):
# call walk to the left till you reach pan = 512 (need while loop)
# while (pan != 512):
print "walk to the left"
elif (pan<headLeftLim):
# call walk to the right till you reach pan = 512 (need while loop)
# while (pan != 512):
print "walk to the left"
if (abs(X)>=100): p=9
elif (abs(X)<100 and abs(X) >70): p = 7
elif (abs(X)<= 70 and abs(X)> 50): p= 5
else: p=2
if (abs(Y)>=100): j=5
elif (abs(Y)<100 and abs(Y) >70): j = 4
elif (abs(Y)<= 70 and abs(Y)> 50): j= 3
else: j=2
print ('p = ',p, 'j= ',j)
if HeadMoved == 0:
i= int(round(X/10))
if (X>50) and (667 > pan):
for I in range(1,z):
api.SetMotorValue(19,pan+p)
elif (X<-50) and (364 < pan):
for I in range(1,abs(z)):
api.SetMotorValue(19,pan-p)
if (Y>50) and ( 612 > tilt):
for I in range(1,z):
api.SetMotorValue(20,tilt+j)
elif (Y<-50) and (433 < tilt):
for I in range(1,abs(z)):
api.SetMotorValue(20,tilt-j)
print Distance
if (Distance < 150) and (Walk == False):
api.Walk(True)
speed = int((150-Distance)/2)
if speed > 10: speed = 10
print ('Speed = ',speed)
api.WalkMove(speed)
Walk = True
sit = 0
HeadMoved = 1
print('Walk=======>')
elif(Distance > 150) and (Walk == True) :
#api.WalkMove(2)
#api.Walk(False)
Walk=False
if (sit == 0):
api.Walk(False)
api.PlayAction(15)
sit = 1
print ( 'Sitting+++++++++')
if (Walk == True):
if (X>50):
api.WalkTurn(p)
#api.WalkMove(Distance)
print('Trun Right')
elif (X<-50):
api.WalkTurn(-p)
print('Trun Left')
else :
api.WalkTurn(0)
print ('No Trun')
print("pan",pan)
print("tilt",tilt)