feat: created modules, logs and removed unuzed functions

.gitignore

@@ -0,0 +1,2 @@
+models
+__pycache__

control.py

@@ -1,87 +1,55 @@
+import logging
 import math
 import numpy as np
-
-
-def sandbox(t):
-    """
-    python simulator.py -m sandbox
-
-    Un premier bac à sable pour faire des expériences
-
-    La fonction reçoit le temps écoulé depuis le début (t) et retourne une position cible
-    pour les angles des 12 moteurs
-
-    - Entrée: t, le temps (secondes écoulées depuis le début)
-    - Sortie: un tableau contenant les 12 positions angulaires cibles (radian) pour les moteurs
-    """
-
-    # Par exemple, on envoie un mouvement sinusoidal
-    targets = [0] * 12
-    targets[0] = t ** 3
-    targets[1] = 0
-    targets[2] = 0
-    targets[3] = t ** 3
-    targets[4] = 0
-    targets[5] = 0
-    targets[6] = t ** 3
-    targets[7] = 0
-    targets[8] = 0
-    targets[9] = t ** 3
-    targets[10] = 0
-    targets[11] = 0
-
-    return targets
-
-
-def direct(alpha, beta, gamma):
-    """
-    python simulator.py -m direct
-
-    Le robot est figé en l'air, on ne contrôle qu'une patte
-
-    Reçoit en argument la cible (alpha, beta, gamma) des degrés de liberté de la patte, et produit
-    la position (x, y, z) atteinte par le bout de la patte
-
-    - Sliders: les angles des trois moteurs (alpha, beta, gamma)
-    - Entrées: alpha, beta, gamma, la cible (radians) des moteurs
-    - Sortie: un tableau contenant la position atteinte par le bout de la patte (en mètres)
-    """
-
-    return 0, 0, 0
-
-
 from math import *
+from logs import *
+from xbox import *
+import time
+
+# DEBUG
+LEGS_LOG_LEVEL = logging.INFO
+CONTROLLER_LOG_LEVEL = logging.INFO
+# Variables configurations
 
 l1h = 0.049
 l1v = 0.032
-l1 = l1h
+l1 = l1h  # this is not the real distance as it's not the one needed to calculate position.
 
+# length between motor 2 and motor 3.
 l2h = 0.0605
 l2v = 0.02215
 l2 = sqrt(l2h ** 2 + l2v ** 2)
 
+# length between motor 3 and end of the leg.
 l3h = 0.012
 l3v = 0.093
 l3 = sqrt(l3h ** 2 + l3v ** 2)
 
+# offset of the 'head', the legs isolated at each end.
 tete_x = 0.095
 
+# offset of the legs at the side.
 patte_y = 0.032
 patte_x = 0.079
+num_patte = 6
+
+# Logs functions
+legsLogger = setup_logger("legs", LEGS_LOG_LEVEL)
+controllerLogger = setup_logger("Controller", CONTROLLER_LOG_LEVEL)
+
+# Initialize controller
+xbox = Xbox(controllerLogger)
+CONTROLLER_MODE = xbox.initialized
+
+
+def interpol2(point2, point1, t):
+    x1, y1, z1 = point1
+    x2, y2, z2 = point2
+    return t * x1 + (1 - t) * x2, t * y1 + (1 - t) * y2, t * z1 + (1 - t) * z2
 
 
 def inverse(x, y, z):
     """
-    python simulator.py -m inverse
-
-    Le robot est figé en l'air, on ne contrôle qu'une patte
-
-    Reçoit en argument une position cible (x, y, z) pour le bout de la patte, et produit les angles
-    (alpha, beta, gamma) pour que la patte atteigne cet objectif
-
-    - Sliders: la position cible x, y, z du bout de la patte
-    - Entrée: x, y, z, une position cible dans le repère de la patte (mètres), provenant du slider
-    - Sortie: un tableau contenant les 3 positions angulaires cibles (en radians)
     """
     # Dimensions (m)
     z += l1v
@@ -112,53 +80,9 @@ def inverse(x, y, z):
     # return [0, angle1 , angle1 -pi/2 + atan(l3h/l3v)]
 
 
-def draw(t):
-    """
-    python simulator.py -m draw
-
-    Le robot est figé en l'air, on ne contrôle qu'une patte
-
-    Le but est, à partir du temps donné, de suivre une trajectoire de triangle. Pour ce faire, on
-    utilisera une interpolation linéaire entre trois points, et la fonction inverse précédente.
-
-    - Entrée: t, le temps (secondes écoulées depuis le début)
-    - Sortie: un tableau contenant les 3 positions angulaires cibles (en radians)
-    """
-
-    def interpol(x2, y2, z2, x1, y1, z1, t):
-        return t * x1 + (1 - t) * x2, t * y1 + (1 - t) * y2, t * z1 + (1 - t) * z2
-
-    p1 = [0.15, 0, 0]
-    p2 = [0.1, 0.1, 0]
-    p3 = [0.1, 0, 0.05]
-
-    positions = [p1, p2, p3]
-
-    ratio = (t % 1)
-    partie = floor((t % 3))
-    partie2 = (partie + 1) % 3
-    # print(f"partie: {partie}, partie2: {partie2}")
-    wanted_origin_x, wanted_origin_y, wanted_origin_z = positions[partie][0], positions[partie][1], positions[partie][
-        2],
-    wanted_dest_x, wanted_dest_y, wanted_dest_z = positions[partie2][0], positions[partie2][1], positions[partie2][2],
-    # print(wanted_origin_x, wanted_origin_y, wanted_origin_z)
-    # print(wanted_dest_x, wanted_dest_y, wanted_dest_z)
-    # print(t)
-    wanted_x, wanted_y, wanted_z = interpol(wanted_origin_x, wanted_origin_y, wanted_origin_z, wanted_dest_x,
-                                            wanted_dest_y, wanted_dest_z, ratio)
-
-    return inverse(wanted_x, wanted_y, wanted_z)
-
-
 def legs(targets_robot):
     """
-    python simulator.py -m legs
+    takes a list of target and offsets it to be in the legs referential
-
-    Le robot est figé en l'air, on contrôle toute les pattes
-
-    - Sliders: les 12 coordonnées (x, y, z) du bout des 4 pattes
-    - Entrée: des positions cibles (tuples (x, y, z)) pour le bout des 4 pattes
-    - Sortie: un tableau contenant les 12 positions angulaires cibles (radian) pour les moteurs
     """
 
     targets = [0] * 18
@@ -187,25 +111,13 @@ def legs(targets_robot):
     return targets
 
 
-def interpol2(point2, point1, t):
-    x1, y1, z1 = point1
-    x2, y2, z2 = point2
-    return t * x1 + (1 - t) * x2, t * y1 + (1 - t) * y2, t * z1 + (1 - t) * z2
-
-
 def walkV1(t, speed_x, speed_y, speed_rotation):
-    """
+    max_slider = 0.200
-    python simulator.py -m walk
+    xboxdata = xbox.get_data()
+    print(xboxdata)
+    speed_x = max_slider * xboxdata["x1"]
+    speed_y = max_slider * xboxdata["y1"]
 
-    Le but est d'intégrer tout ce que nous avons vu ici pour faire marcher le robot
-
-    - Sliders: speed_x, speed_y, speed_rotation, la vitesse cible du robot
-    - Entrée: t, le temps (secondes écoulées depuis le début)
-            speed_x, speed_y, et speed_rotation, vitesses cibles contrôlées par les sliders
-    - Sortie: un tableau contenant les 12 positions angulaires cibles (radian) pour les moteurs
-    """
-    # t = t*speed_x * 20
-    num_patte = 6
     slider_max = 0.200
 
     neutral_position = np.array([

logs.py

@@ -0,0 +1,47 @@
+import logging
+
+
+class ColorFormatter(logging.Formatter):
+    """Custom formatter with colored output based on log level"""
+
+    # ANSI color codes
+    GREY = "\x1b[38;21m"
+    GREEN = "\x1b[92m"
+    YELLOW = "\x1b[93m"
+    RED = "\x1b[91m"
+    BOLD_RED = "\x1b[31;1m"
+    RESET = "\x1b[0m"
+
+    # Format including function name
+    FORMAT = "%(asctime)s | %(funcName)s | %(message)s"
+
+    FORMATS = {
+        logging.DEBUG: GREY + FORMAT + RESET,
+        logging.INFO: GREEN + FORMAT + RESET,
+        logging.WARNING: YELLOW + FORMAT + RESET,
+        logging.ERROR: RED + FORMAT + RESET,
+        logging.CRITICAL: BOLD_RED + FORMAT + RESET
+    }
+
+    def format(self, record):
+        log_format = self.FORMATS.get(record.levelno)
+        formatter = logging.Formatter(log_format)
+        return formatter.format(record)
+
+
+def setup_logger(name, level=logging.INFO):
+    """Set up a logger with the custom color formatter"""
+    logger = logging.getLogger(name)
+    logger.setLevel(level)
+
+    # Create console handler
+    console_handler = logging.StreamHandler()
+    console_handler.setLevel(level)
+
+    # Add formatter to console handler
+    console_handler.setFormatter(ColorFormatter())
+
+    # Add console handler to logger
+    logger.addHandler(console_handler)
+
+    return logger

xbox.py

@@ -0,0 +1,50 @@
+import pygame
+
+
+class Xbox:
+    def __init__(self, controllerLogger):
+
+        pygame.init()
+        self.controllers = []
+        for i in range(0, pygame.joystick.get_count()):
+            self.controllers.append(pygame.joystick.Joystick(i))
+            self.controllers[-1].init()
+            controllerLogger.info(f"Detected controller {self.controllers[-1].get_name()}")
+
+        if len(self.controllers) == 0:
+            controllerLogger.critical("No controllers detected. Can't initialize remote control.")
+            self.initialized = True
+        else:
+            self.initialized = False
+        self.data = {"x1": 0, "x2": 0, "y1": 0, "y2": 0, "up": 0, "down": 0, "left": 0, "right": 0, "r1": 0, "r2": 0,
+                     "r3": 0,
+                     "l1": 0, "l2": 0, "l3": 0}
+
+    def get_data(self):
+        for event in pygame.event.get():
+            event = dict(event.dict)
+            keys = event.keys()
+            try:
+                if "axis" in keys:
+                    axis = event["axis"]
+                    if axis in [1, 4]:
+                        value = -event["value"]
+                        if abs(value) < 0.1:
+                            value = 0
+                        self.data[{0: "x1", 1: "y1", 4: "y2", 3: "x2", 5: "r2", 2: "l2"}[event["axis"]]] = value
+                    else:
+                        value = event["value"]
+                        if abs(value) < 0.1:
+                            value = 0
+                        self.data[{0: "x1", 1: "y1", 4: "y2", 3: "x2", 5: "r2", 2: "l2"}[event["axis"]]] = value
+                elif "button" in keys:
+                    # TODO
+                    pass
+                else:
+                    print(event)
+
+            except Exception as e:
+                print(event)
+                print(e)
+
+        return self.data