feat: created modules, logs and removed unuzed functions
This commit is contained in:
Pierre Tellier
2
.gitignore
vendored
Normal file
2
.gitignore
vendored
Normal file
@@ -0,0 +1,2 @@
|
||||
models
|
||||
__pycache__
|
||||
158
control.py
158
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([
|
||||
|
||||
47
logs.py
Normal file
47
logs.py
Normal file
@@ -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
|
||||
50
xbox.py
Normal file
50
xbox.py
Normal file
@@ -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
|
||||
Reference in New Issue
Block a user