def main():
# start socket
TCP_IP = '128.237.198.49'
TCP_PORT = 2002
print('Socket Information: %s:%d' % (TCP_IP, TCP_PORT))
s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
s.connect((TCP_IP, TCP_PORT))
time.sleep(1e-3)
# start camera
vs = VideoStream(src=0).start()
time.sleep(2.0)
# calibration and find block
#Caliberate_camera(vs)
block_pixel_position = detect_block_grab(vs)
block_real_position = transfer_to_real(block_pixel_position)
print(block_pixel_position)
print(block_real_position)
# Inverse Kinematics
inverse_kinematics(block_real_position,s)
HOME_POSITION = [20,-15,20]
roll = -math.pi/2
traj = RPC.pipline_position_encoder_roll(HOME_POSITION, [40,-30,20], roll, s)
traj = RPC.pipline_position_encoder_roll([40,-30,20], [40,-30,12], roll, s)
Pipeline.C_execute([traj])
the_block = [42, -26, 1, -1.04]
ball_position = detect_ball(vs)
ball_position.reverse()
K = 0.4
adj = 0
for pos in ball_position:
if pos == None:
continue
print(pos)
if pos[1] > -16:
if pos[0] > 47:
err = pos[0]-47
adj = err*K
break
print(adj)
#print(ball_position)
commands = Pipeline.Adjust(the_block, adj, s)
for command in commands:
print(command)
Pipeline.C_execute(commands)
'
