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

feat: added sliders back

Browse Source
This commit is contained in:
Pierre Tellier 2025-05-04 22:30:57 +02:00
parent 561b2f70a2
commit 248e8dd073
2 changed files with 41 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

73
control.py
View File

@ -44,19 +44,19 @@ CONTROLLER_MODE = xbox.initialized
xbox.mode_count = 5 xbox.mode_count = 5
neutral_position = np.array([ neutral_position = np.array([
[0.1, 0.16, -0.15], [0.08, 0.16, -0.12],
[-0.1, 0.16, -0.15], [-0.08, 0.16, -0.12],
[-0.2, -0.00, -0.15], [-0.2, -0.00, -0.12],
[-0.1, -0.16, -0.15], [-0.08, -0.16, -0.12],
[0.1, -0.16, -0.15], [0.08, -0.16, -0.12],
[0.2, 0, -0.15] [0.2, 0, -0.12]
]) ])
REAL = 0 REAL = 0
# Robot setup # Robot setup
if REAL == 1: if REAL == 1:
portHandler = PortHandler("/dev/ttyUSB0") portHandler = PortHandler("/dev/ttyUSB1")
packetHandler = PacketHandler(1.0) packetHandler = PacketHandler(1.0)
portHandler.openPort() portHandler.openPort()
@ -71,6 +71,9 @@ if REAL == 1:
print(f"Found AX-12 with id: {id}") print(f"Found AX-12 with id: {id}")
ids.append(id) ids.append(id)
time.sleep(0.01) time.sleep(0.01)
if len(ids) != 18:
legsLogger.error("Not all motor found. Press enter to continue")
input()
# Done # Done
@ -268,30 +271,33 @@ def stand(x1, y1, x2, y2):
def walk(t, sx, sy, sr): def walk(t, sx, sy, sr):
xboxdata = xbox.get_data() xboxdata = xbox.get_data()
val = xboxdata["x1"] if xbox.initialized:
max_slider = 0.200
controllerLogger.debug(xboxdata)
if xbox.mode == 0:
positions = static()
elif xbox.mode == 1:
positions = jump(xboxdata["y2"])
elif xbox.mode == 2:
positions = dino_naive(t, max_slider * xboxdata["y1"], max_slider * xboxdata["x1"],
max_slider * xboxdata["y2"],
max_slider * xboxdata["x2"], max_slider * (xboxdata["r2"] + 1))
elif xbox.mode == 3:
positions = naive_walk(t, max_slider * xboxdata["x1"], max_slider * xboxdata["y1"])
elif xbox.mode == 4:
positions = walk_v2(t, max_slider * xboxdata["y1"], -1 * max_slider * xboxdata["x1"],
xboxdata["x2"] * max_slider,
max_slider * xboxdata["y2"])
elif xbox.mode == 5:
positions = stand(xboxdata["x1"], xboxdata["y1"], xboxdata["x2"], xboxdata["y2"])
max_slider = 0.200 # print(positions)
controllerLogger.debug(xboxdata) if REAL == 1:
if xbox.mode == 0: convert_to_robot(positions)
positions = static()
elif xbox.mode == 1:
positions = jump(xboxdata["y2"])
elif xbox.mode == 2:
positions = dino_naive(t, max_slider * xboxdata["y1"], max_slider * xboxdata["x1"], max_slider * xboxdata["y2"],
max_slider * xboxdata["x2"], max_slider * (xboxdata["r2"] + 1))
elif xbox.mode == 3:
positions = naive_walk(t, max_slider * xboxdata["x1"], max_slider * xboxdata["y1"])
elif xbox.mode == 4:
positions = dev(t, max_slider * xboxdata["y1"], -1 * max_slider * xboxdata["x1"], xboxdata["x2"] * max_slider,
max_slider * xboxdata["y2"])
elif xbox.mode == 5:
positions = stand(xboxdata["x1"], xboxdata["y1"], xboxdata["x2"], xboxdata["y2"])
print(positions) return positions
if REAL == 1: else:
convert_to_robot(positions) return walk_v2(t, sx, sy, sr, None)
return positions
def static(): def static():
@ -299,14 +305,13 @@ def static():
# Walk V2 # Walk V2
def dev(t, speed_x, speed_y, speed_r, y2): def walk_v2(t, speed_x, speed_y, speed_r, y2):
""" """
x1: speed along the x-axis x1: speed along the x-axis
y1: speed along the y-axis y1: speed along the y-axis
x2: rotational speed along the z-axis x2: rotational speed along the z-axis
""" """
max_dist = 0.3 max_dist = 0.3
print(speed_r)
def get_rotation_center(speed_x, speed_y, theta_point): def get_rotation_center(speed_x, speed_y, theta_point):
norm = sqrt(speed_x ** 2 + speed_y ** 2) norm = sqrt(speed_x ** 2 + speed_y ** 2)
@ -324,9 +329,9 @@ def dev(t, speed_x, speed_y, speed_r, y2):
real_position = np.copy(neutral_position) real_position = np.copy(neutral_position)
movement_z = np.array([ movement_z = np.array([
[0, 0, 0.02], [0, 0, 0.03],
[0, 0, -0.01], [0, 0, -0.02],
[0, 0, -0.01] [0, 0, -0.02]
]) ])
step_duration = np.array([0.15, 0.3, 0.15]) step_duration = np.array([0.15, 0.3, 0.15])

4
xbox.py
View File

@ -15,9 +15,9 @@ class Xbox:
if len(self.controllers) == 0: if len(self.controllers) == 0:
self.logger.critical("No controllers detected. Can't initialize remote control.") self.logger.critical("No controllers detected. Can't initialize remote control.")
self.initialized = True
else:
self.initialized = False self.initialized = False
else:
self.initialized = True
self.data = {"x1": 0, "x2": 0, "y1": 0, "y2": 0, "up": 0, "down": 0, "left": 0, "right": 0, "r1": 0, "r2": 0, self.data = {"x1": 0, "x2": 0, "y1": 0, "y2": 0, "up": 0, "down": 0, "left": 0, "right": 0, "r1": 0, "r2": 0,
"r3": 0, "r3": 0,
"l1": 0, "l2": 0, "l3": 0} "l1": 0, "l2": 0, "l3": 0}