piair/vroom-vroom-vroom
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat: added x and y movement

Browse Source
This commit is contained in:
Pierre Tellier 2025-03-31 19:50:14 +02:00
parent 55fa408d62
commit ed7915386d
1 changed files with 17 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
control.py
View File

@ -205,38 +205,39 @@ def walkV1(t, speed_x, speed_y, speed_rotation):
- Sortie: un tableau contenant les 12 positions angulaires cibles (radian) pour les moteurs - Sortie: un tableau contenant les 12 positions angulaires cibles (radian) pour les moteurs
""" """
# t = t*speed_x * 20 # t = t*speed_x * 20
num_patte = 4 num_patte = 6
slider_max = 0.200 slider_max = 0.200
neutral_position = np.array([ neutral_position = np.array([
[0.1, 0.15, -0.16], [0.1, 0.15, -0.15],
[-0.1, 0.15, -0.16], [-0.1, 0.15, -0.15],
[-0.2, -0.00, -0.16], [-0.2, -0.00, -0.15],
[-0.1, -0.15, -0.16], [-0.1, -0.15, -0.15],
[0.1, -0.15, -0.16], [0.1, -0.15, -0.15],
[0.2, 0, -0.16] [0.2, 0, -0.15]
]) ])
real_position = np.copy(neutral_position) real_position = np.copy(neutral_position)
movement_x = np.array([ movement_x = np.array([
[0.0, 0, 0],
[0.00, 0, 0], [0.00, 0, 0],
[-0.00, 0, 0], [0.04, 0, 0],
[-0.04, 0, 0],
]) ])
movement_y = np.array([ movement_y = np.array([
[0.0, 0, 0], [0.0, 0, 0],
[0, 0.00, 0], [0, 0.04, 0],
[0, -0.00, 0], [0, -0.04, 0],
]) ])
movement_z = np.array([ movement_z = np.array([
[0, 0, 0], [0, 0, 0.08],
[0, 0, 0], [0, 0, -0.02],
[0, 0, 0] [0, 0, -0.02]
]) ])
# duration of each step of the movement
step_duration = np.array([0.05, 0.3, 0.05]) step_duration = np.array([0.05, 0.3, 0.05])
step_count = len(movement_z) step_count = len(movement_z)
@ -263,13 +264,13 @@ def walkV1(t, speed_x, speed_y, speed_rotation):
return floor((get_current_step(t) + 1) % step_count) return floor((get_current_step(t) + 1) % step_count)
def rotate(patte): def rotate(patte):
return [1, -1, -1, 1][ return [1, -1, 0, -1, 1, 0][
patte] * movement_x # + [-1, 1, -1, 1][patte] * movement_y # mettre des 0 partout sur le Y fait une très belle rotation patte] * movement_x # + [-1, 1, -1, 1][patte] * movement_y # mettre des 0 partout sur le Y fait une très belle rotation
def normalize(matrix, slider_max, speed): def normalize(matrix, slider_max, speed):
return (matrix / slider_max) * speed return (matrix / slider_max) * speed
offsets = np.array([0, 0.5, 0, 0.5]) * movement_duration # offset between each leg offsets = np.array([0, 1 / 3, 2 / 3, 0, 1 / 3, 2 / 3]) * movement_duration # offset between each leg
assert len(offsets) == num_patte, f"all offsets must be set, currently, {len(offsets)}/{num_patte} have them" assert len(offsets) == num_patte, f"all offsets must be set, currently, {len(offsets)}/{num_patte} have them"
for patte in range(num_patte): for patte in range(num_patte):