def main():
#Setup
sock = simplesocket.simplesocket(12345,"192.168.1.84")
#Mainloop
while True:
#print "Sending \"%s\" and then waiting %d seconds"%(stringToSend,executeTime+captureDelayTime)
#sock.send(stringToSend)
received,buf = sock.receive()
if received:
print "Received:%r"%buf
time.sleep(0.1)
import sys import cv2.cv as cv from optparse import OptionParser import serial import simplesocket import time import math sock=simplesocket.simplesocket(12345) comOut = serial.Serial(port='/dev/ttyUSB0',baudrate=115200) # Parameters for haar detection # From the API: # The default parameters (scale_factor=2, min_neighbors=3, flags=0) are tuned # for accurate yet slow object detection. For a faster operation on real video # images the settings are: # scale_factor=1.2, min_neighbors=2, flags=CV_HAAR_DO_CANNY_PRUNING, # min_size=<minimum possible face size min_size = (20, 20) image_scale = 1 haar_scale = 1.2 min_neighbors = 2 haar_flags = 0 pan=0 tilt=0 prevpanchange=0 prevtiltchange=0
def start_socket():
global ssocket
ssocket = simplesocket(HOST, PORT, receive_message, socket_goneoff)
ssocket.start()
def main(enable_gamepad=True):
#Setup
motionsock = simplesocket.simplesocket(12345)
if enable_gamepad:
gamepadsock = simplesocket.simplesocket(gamepad_helper.GP_PORT)
cap = setupCapture()
analysisimg = numpy.zeros((200,200,4),numpy.uint8)
surface = cairo.ImageSurface.create_for_data(analysisimg,cairo.FORMAT_ARGB32,200,200)
cr = cairo.Context(surface)
captureDelayTime = 1 #estimated 2 second delay in capturing image
lastbutton1 = False
lastbutton2 = False
currentGait = 0
servosPowered = False
if enable_gamepad:
#Mainloop
while True:
received,buf = gamepadsock.receive()
if received:
print "Received:%r"%buf
print "Axisvalues: %3d,%3d,%3d,%3d,%3d"%(ord(buf[0]),ord(buf[1]),ord(buf[2]),ord(buf[3]),ord(buf[4]))
print "Buttonvalues: %2d,%2d,%2d,%2d, %2d,%2d,%2d,%2d, %2d,%2d"%(ord(buf[5]),ord(buf[6]),ord(buf[7]),ord(buf[8]), ord(buf[9]),ord(buf[10]),ord(buf[11]),ord(buf[12]), ord(buf[13]),ord(buf[14]))
stringToSend = ""
button0 = (ord(buf[5]) != 0)
button1 = (ord(buf[6]) != 0)
button2 = (ord(buf[7]) != 0)
if button2:
if not lastbutton2:
lastbutton2 = True
servosPowered = not servosPowered
stringToSend = "P %d"%(1 if servosPowered else 0)
else:
lastbutton2 = False
if stringToSend == "":
if button0:
legMoveX = ord(buf[0])/32.0-2.0 #-2 .. 6
legMoveY = ord(buf[2])/32.0-6.0 #-6 .. 2
legMoveZ = ord(buf[1])/32.0-1.0 #-1 .. 7
stringToSend = "S 4 %g %g %g"%(legMoveX,legMoveY,legMoveZ)
else:
speedx = ord(buf[0])/-128.0+1.0
speedy = ord(buf[1])/-64.0+2.0
print "Speed x,y: (%g,%g)"%(speedx,speedy)
if math.hypot(speedx,speedy)>0.4:
stringToSend = "W %g %g %g %d"%(speedx,speedy,0,1*25)
else:
rotx = (ord(buf[3])/128.0-1.0)*0.25
roty = 0
rotz = (ord(buf[4])/128.0-1.0)*0.25
height = ord(buf[2])/128.0-1.0
stringToSend = "R %g %g %g %g"%(rotx,roty,rotz,height)
if button1:
if not lastbutton1:
lastbutton1 = True
currentGait += 1
if currentGait > 2:
currentGait = 0
stringToSend = "G %d"%currentGait
else:
lastbutton1 = False
print "stringToSend: '%s'"%stringToSend
motionsock.send(stringToSend)
else:
#Analyze camera picture
ret, img = cap.read()
print "cap.read():"+str(ret)
x,z,r = analyzeArMarkersInImage(img)
print "x:%r, z:%r, r:%r"%(x,z,r)
if not x is None:
#Send new walking command
dr = 0 #sorted((-0.1, r/10, 0.1))[1]
dx = sorted((-0.5, x/10, 0.5))[1]
dy = sorted((-1, (z-60)/10, 1))[1]
executeTime = 2
ticks = executeTime * 25 #50 ms sleep per tick
stringToSend = "W %g %g %g %d"%(dx,dy,dr,ticks)
print "Sending \"%s\" and then waiting %d seconds"%(stringToSend,executeTime+captureDelayTime)
motionsock.send(stringToSend)
#Wait until timer expires and check socket receiving in meantime
timeToWaitUntil = time.time() + executeTime
while time.time()<timeToWaitUntil:
#received,buf = motionsock.receive()
#if received:
#print "Received:%r"%buf
time.sleep(0.1)
timeToWaitUntil = time.time() + captureDelayTime
while time.time()< timeToWaitUntil:
ret,img = cap.read()
print "Sleeping after command done"
else:
print "No markers detected"
pygame.display.set_caption("My Game")
#Loop until the user clicks the close button.
done = False
# Used to manage how fast the screen updates
clock = pygame.time.Clock()
# Initialize the joysticks
pygame.joystick.init()
# Get ready to print
textPrint = TextPrint()
sock = simplesocket.simplesocket(gamepad_helper.GP_PORT,gamepad_helper.TAU_IP)
# -------- Main Program Loop -----------
while done==False:
# EVENT PROCESSING STEP
for event in pygame.event.get(): # User did something
if event.type == pygame.QUIT: # If user clicked close
done=True # Flag that we are done so we exit this loop
# Possible joystick actions: JOYAXISMOTION JOYBALLMOTION JOYBUTTONDOWN JOYBUTTONUP JOYHATMOTION
if event.type == pygame.JOYBUTTONDOWN:
print("Joystick button pressed.")
if event.type == pygame.JOYBUTTONUP:
print("Joystick button released.")
def main():
#Setup
motionsock = simplesocket.simplesocket(12345)
gamepadsock = simplesocket.simplesocket(gamepad_helper.GP_PORT)
cap = setupCapture()
analysisimg = numpy.zeros((200,200,4),numpy.uint8)
surface = cairo.ImageSurface.create_for_data(analysisimg,cairo.FORMAT_ARGB32,200,200)
cr = cairo.Context(surface)
captureDelayTime = 1 #estimated 2 second delay in capturing image
lastbutton1 = False
lastbutton2 = False
currentGait = 0
servosPowered = False
#Mainloop
while True:
received,buf = gamepadsock.receive()
if received:
print "Received:%r"%buf
print "Axisvalues: %3d,%3d,%3d,%3d,%3d"%(ord(buf[0]),ord(buf[1]),ord(buf[2]),ord(buf[3]),ord(buf[4]))
print "Buttonvalues: %2d,%2d,%2d,%2d, %2d,%2d,%2d,%2d, %2d,%2d"%(ord(buf[5]),ord(buf[6]),ord(buf[7]),ord(buf[8]), ord(buf[9]),ord(buf[10]),ord(buf[11]),ord(buf[12]), ord(buf[13]),ord(buf[14]))
stringToSend = ""
button0 = (ord(buf[5]) != 0)
button1 = (ord(buf[6]) != 0)
button2 = (ord(buf[7]) != 0)
if button2:
if not lastbutton2:
lastbutton2 = True
servosPowered = not servosPowered
stringToSend = "P %d"%(1 if servosPowered else 0)
else:
lastbutton2 = False
if stringToSend == "":
if button0:
legMoveX = ord(buf[0])/32.0-2.0 #-2 .. 6
legMoveY = ord(buf[2])/32.0-6.0 #-6 .. 2
legMoveZ = ord(buf[1])/32.0-1.0 #-1 .. 7
stringToSend = "S 4 %g %g %g"%(legMoveX,legMoveY,legMoveZ)
else:
speedx = ord(buf[0])/-128.0+1.0
speedy = ord(buf[1])/-64.0+2.0
print "Speed x,y: (%g,%g)"%(speedx,speedy)
if math.hypot(speedx,speedy)>0.4:
stringToSend = "W %g %g %g %d"%(speedx,speedy,0,1*25)
else:
rotx = (ord(buf[3])/128.0-1.0)*0.25
roty = 0
rotz = (ord(buf[4])/128.0-1.0)*0.25
height = ord(buf[2])/128.0-1.0
stringToSend = "R %g %g %g %g"%(rotx,roty,rotz,height)
if button1:
if not lastbutton1:
lastbutton1 = True
currentGait += 1
if currentGait > 2:
currentGait = 0
stringToSend = "G %d"%currentGait
else:
lastbutton1 = False
print "stringToSend: '%s'"%stringToSend
motionsock.send(stringToSend)
