Rpi

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()

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]

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

scripts/eyes.html

@@ -0,0 +1,109 @@
+<!DOCTYPE html>
+<html lang="en">
+
+<head>
+  <meta charset="UTF-8">
+  <meta http-equiv="X-UA-Compatible" content="IE=edge">
+  <meta name="viewport" content="width=device-width, initial-scale=1.0">
+  <title>Eyes Follow Mouse Cursor</title>
+
+  <style>
+    body {
+      background-color: yellow;
+      margin: 0;
+      padding: 0;
+      grid-template-columns: 1fr;
+      grid-template-rows: 100vh;
+      display: grid;
+      align-items: center;
+      justify-items: center;
+    }
+
+    .eyesContainer {
+      width: 200px;
+      height: 200px;
+      background-color: tomato;
+      display: grid;
+      align-items: center;
+      justify-items: center;
+      grid-template-columns: 1fr 1fr;
+      border-radius: 10px;
+      transform-style: preserve-3d;
+      box-shadow: 0 30px 70px #666;
+      will-change: transform;
+      transform: perspective(500px) rotateX(var(--yAngle, 0deg)) rotateY(var(--xAngle, 0deg));
+    }
+
+    .eye {
+      width: 50px;
+      height: 50px;
+      border-radius: 50%;
+      background-color: white;
+      transform: translateZ(50px) rotate(var(--eyeAngle, 0deg));
+    }
+
+    .pupil {
+      position: relative;
+      width: 16px;
+      height: 16px;
+      background: #000;
+      border-radius: 50%;
+      top: calc(50% - 8px);
+      left: 5px;
+    }
+  </style>
+</head>
+
+<body>
+  <div class="eyesContainer">
+    <div class="eye">
+      <div class="pupil"></div>
+    </div>
+    <div class="eye">
+      <div class="pupil"></div>
+    </div>
+  </div>
+  <script>
+
+    let eyes = document.querySelectorAll(".eye");
+
+    let eyeRect = eyes[0].getBoundingClientRect();
+
+    let container = document.querySelector(".eyesContainer");
+    let containerRect = container.getBoundingClientRect();
+
+    window.addEventListener("resize", updatePosition, false);
+
+    function updatePosition(e) {
+      eyeRect = eyes[0].getBoundingClientRect();
+      containerRect = container.getBoundingClientRect();
+    }
+
+    document.body.addEventListener("mousemove", eyesFollow, false);
+
+    function eyesFollow(e) {
+      requestAnimationFrame(() => {
+        let xPos = e.pageX;
+        let yPos = e.pageY;
+
+        let xDiff = (eyeRect.x + eyeRect.width / 2) - xPos;
+        let yDiff = (eyeRect.y + eyeRect.height / 2) - yPos;
+
+        let angle = Math.atan2(yDiff, xDiff) * 180 / Math.PI;
+
+        container.style.setProperty("--eyeAngle", angle.toFixed(2) + "deg");
+
+        let mouseXRelativetoContainer = xPos - containerRect.x - containerRect.width / 2;
+        let mouseYRelativetoContainer = yPos - containerRect.y - containerRect.height / 2;
+
+        let containerXAngle = 60 * (mouseXRelativetoContainer / window.innerWidth);
+        let containerYAngle = -1 * 60 * (mouseYRelativetoContainer / window.innerHeight);
+
+        container.style.setProperty("--xAngle", containerXAngle.toFixed(2) + "deg");
+        container.style.setProperty("--yAngle", containerYAngle.toFixed(2) + "deg");
+      });
+    }
+  </script>
+</body>
+
+</html>

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)