diff --git a/scripts/core/protocol.py b/scripts/core/protocol.py index d2ba255..eaa6215 100644 --- a/scripts/core/protocol.py +++ b/scripts/core/protocol.py @@ -27,7 +27,7 @@ def listen(self, port): while True: # Get socket. server = socket.socket(socket.AF_INET, socket.SOCK_STREAM) - server.bind((socket.gethostname(), port)) + server.bind(('', port)) print(f"Listening on {socket.gethostname()}:{port}") server.listen() (client, (ip, _)) = server.accept() diff --git a/scripts/core/state.py b/scripts/core/state.py index 2ff19ee..7bb79ae 100644 --- a/scripts/core/state.py +++ b/scripts/core/state.py @@ -1,4 +1,5 @@ from enum import Enum +from adafruit_servokit import ServoKit class Servo(Enum): """Enum for identifying servos.""" @@ -11,7 +12,7 @@ class State: """Holds the state of the robot's servos.""" def __init__(self): - self.positions = [0 for servo in Servo] + self.positions = ServoKit(channels=16) def lookAt(self, point: (float, float, float)): """ @@ -47,8 +48,9 @@ def update(self, target, deltaT: float): def setPosition(self, servo, position): """Sets the position of a servo.""" - self.positions[servo.value] = position + self.positions.servo[servo.value] = position def getPosition(self, servo): """Gets the position of a servo.""" - return self.positions[servo.value] + #TODO: na documentação não mencionam a leitura do ângulo, por isso não sei se isto funciona + return self.positions.servo[servo.value] diff --git a/scripts/core/window.py b/scripts/core/window.py new file mode 100644 index 0000000..27df37d --- /dev/null +++ b/scripts/core/window.py @@ -0,0 +1,78 @@ +import pygame +import math +from .state import Servo + +LEFT_EYE_CENTER = (320,400) +RIGHT_EYE_CENTER = (480,400) + +EYE_RADIUS = 50 +PUPIL_RADIUS = 15 + +PUPIL_LIMIT = 30 # EYE_RADIUS - PUPIL_RADIUS - 5 Pixels, so the pupil is not in the limit of the eye + +ANGLE_OF_STATE_UNIT = EYE_RADIUS / 180 + +Z_PLANE = 1000 + + +def draw_eye(win,eye_x, eye_y, robot, position): + new_x, new_y = pygame.mouse.get_pos() # Gets mouse position + + #TODO: State needs to be initialized when the program runs, + # otherwhise it is not possible to run the scrip bellow + + """ + robot_state = robot.getActualState() + # Checks which eye it is, and updates the new position of the pupil + # This new position is based on the angle given to the servos of the + # robot + + if position == "left": + new_x = robot_state.getPosition(Servo.L_EYE_X) * ANGLE_OF_STATE_UNIT + new_y = robot_state.getPosition(Servo.L_EYE_Y) * ANGLE_OF_STATE_UNIT + if position == "right": + new_x = robot_state.getPosition(Servo.R_EYE_X) * ANGLE_OF_STATE_UNIT + new_y = robot_state.getPosition(Servo.R_EYE_Y) * ANGLE_OF_STATE_UNIT + else: + print("Error, that eye does not exist") + + """ + + distance_x = new_x - eye_x + distance_y = new_y - eye_y + + """" + Angle between plane and line: + + |u.n| + sin(tetha) = ------------ + |u|.|n| + + In our case n = (0,0,1) and u = (distance_x, distance_y, Z_PLANE) + so |u.n| = uz.nz = Z_PLANE + and + |u|.|n| = |u| + + """ + #NOTE: It may seem that there is no plane on the mouse but that's + # because of the dimensions are small, so we cant add a great distance + # to the mouse. + + norm_u = math.sqrt(distance_x**2 + distance_y**2 + Z_PLANE**2) + calc_aux = Z_PLANE/norm_u + theta = math.asin(calc_aux) # in radians + true_distance = norm_u * math.cos(theta) + + distance = min(true_distance, PUPIL_LIMIT) + + angle = math.atan2(distance_y, distance_x) # angle for rotation + pupil_x = eye_x + (math.cos(angle) * distance) # position of pupil of the eye in axis X + pupil_y = eye_y + (math.sin(angle) * distance) # position of pupil of the eye in axis Y + + pygame.draw.circle(surface=win, center=(eye_x, eye_y), radius=EYE_RADIUS, color=(255, 255, 255)) # draw eye + pygame.draw.circle(surface=win, center=(pupil_x, pupil_y), radius=PUPIL_RADIUS, color=(0, 255, 255)) # draw pupil + +def draw_face(win,robot): + pygame.draw.rect(surface=win, color=(255,229,204), rect= (250, 250, 300, 300) ) + draw_eye(win,320, 400, robot, "left") # left eye + draw_eye(win,480, 400, robot, "right") # right eye diff --git a/scripts/eyes.html b/scripts/eyes.html new file mode 100644 index 0000000..dc74f9d --- /dev/null +++ b/scripts/eyes.html @@ -0,0 +1,109 @@ + + + + + + + + Eyes Follow Mouse Cursor + + + + + +
+
+
+
+
+
+
+
+ + + + \ No newline at end of file diff --git a/scripts/main.py b/scripts/main.py index 0ab6881..2ae2e2a 100644 --- a/scripts/main.py +++ b/scripts/main.py @@ -1,13 +1,14 @@ -from core import Parser, Robot, Protocol +from core import Parser, Robot, Protocol, window from constants import UPDATE_RATE from control import MimicBehaviour +import pygame + import cv2 import time def run(options): """The main entry point for the controller.""" - robot = Robot() protocol = Protocol(not options["viewer"], not options["receiver"]) behaviour = MimicBehaviour() @@ -21,11 +22,19 @@ def run(options): else: protocol.listen(options["port"]) elif options["receiver"] or options["viewer"]: - raise RuntimeError("In single mode state and frames can't be received.") + #raise RuntimeError("In single mode state and frames can't be received.") + pass if not options["viewer"]: cap = cv2.VideoCapture(0) + if options["window"]: + run = True + pygame.init() + + win = pygame.display.set_mode((800,800)) + pygame.display.set_caption("INAR Simulator") + # Then, run the main loop. start = time.time() acc = 0 @@ -60,6 +69,21 @@ def run(options): if options["servos"]: pass # TODO: update the servos. + if options["window"]: + pygame.time.delay(100) + + for event in pygame.event.get(): + if event.type == pygame.QUIT: + print("Quitting... Sya :)") + run = False + + if not run: + break + win.fill((255,255,255)) # Fills with the colors inside + window.draw_face(win, robot) + pygame.display.update() + + if __name__ == "__main__": parser = Parser() parser.add("help", "Prints this help message.", default=False) diff --git a/scripts/meshes/eye.stl b/scripts/meshes/eye.stl new file mode 100644 index 0000000..df96172 Binary files /dev/null and b/scripts/meshes/eye.stl differ diff --git a/scripts/meshes/new_eye.stl b/scripts/meshes/new_eye.stl new file mode 100644 index 0000000..df1e647 Binary files /dev/null and b/scripts/meshes/new_eye.stl differ diff --git a/scripts/meshes/teapot.stl b/scripts/meshes/teapot.stl new file mode 100644 index 0000000..d225918 Binary files /dev/null and b/scripts/meshes/teapot.stl differ