From 8479dec3459c4eb90d9bf3dbf7f673436116a0cd Mon Sep 17 00:00:00 2001 From: Pierre Tellier Date: Sat, 5 Apr 2025 21:17:30 +0200 Subject: [PATCH] feat: created modules, logs and removed unuzed functions --- .gitignore | 2 + control.py | 158 ++++++++++++----------------------------------------- logs.py | 47 ++++++++++++++++ xbox.py | 50 +++++++++++++++++ 4 files changed, 134 insertions(+), 123 deletions(-) create mode 100644 .gitignore create mode 100644 logs.py create mode 100644 xbox.py diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..10a149b --- /dev/null +++ b/.gitignore @@ -0,0 +1,2 @@ +models +__pycache__ \ No newline at end of file diff --git a/control.py b/control.py index 75f9130..a580dac 100644 --- a/control.py +++ b/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 - - 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 + takes a list of target and offsets it to be in the legs referential """ 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): - """ - python simulator.py -m walk + max_slider = 0.200 + 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([ diff --git a/logs.py b/logs.py new file mode 100644 index 0000000..5b72c00 --- /dev/null +++ b/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 diff --git a/xbox.py b/xbox.py new file mode 100644 index 0000000..dafcf48 --- /dev/null +++ b/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