rafaelcaricio/Open_Duck_Mini_Runtime
1
0
Fork 0
mirror of https://github.com/apirrone/Open_Duck_Mini_Runtime.git synced 2025-09-02 19:23:54 +00:00
Code Issues Projects Releases Packages Wiki Activity Actions

fix controller

Browse source
This commit is contained in:
apirrone 2025-03-11 16:12:04 +01:00
parent 20f8135415
commit 0c8c880a77
2 changed files with 77 additions and 58 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

116
mini_bdx_runtime/mini_bdx_runtime/xbox_controller.py
View file

@ -24,8 +24,8 @@ class XBoxController:
self.head_control_mode = self.standing
self.last_commands = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
self.last_left_trigger = 0
self.last_right_trigger = 0
self.last_left_trigger = 0.0
self.last_right_trigger = 0.0
pygame.init()
self.p1 = pygame.joystick.Joystick(0)
self.p1.init()
@ -45,72 +45,72 @@ class XBoxController:
last_commands = self.last_commands
left_trigger = self.last_left_trigger
right_trigger = self.last_right_trigger
for event in pygame.event.get():
l_x = -1 * self.p1.get_axis(0)
l_y = -1 * self.p1.get_axis(1)
r_x = -1 * self.p1.get_axis(2)
r_y = -1 * self.p1.get_axis(3)
right_trigger = (self.p1.get_axis(4) + 1) / 2
left_trigger = (self.p1.get_axis(5) + 1) / 2
if left_trigger < 0.1:
left_trigger = 0
if right_trigger < 0.1:
right_trigger = 0
# print(f"Right trigger: {right_trigger}"
# f"Left trigger: {left_trigger}")
l_x = -1 * self.p1.get_axis(0)
l_y = -1 * self.p1.get_axis(1)
r_x = -1 * self.p1.get_axis(2)
r_y = -1 * self.p1.get_axis(3)
if not self.head_control_mode:
lin_vel_y = l_x
lin_vel_x = l_y
ang_vel = r_x
if lin_vel_x >= 0:
lin_vel_x *= np.abs(X_RANGE[1])
else:
lin_vel_x *= np.abs(X_RANGE[0])
right_trigger = np.around((self.p1.get_axis(4) + 1) / 2, 3)
left_trigger = np.around((self.p1.get_axis(5) + 1) / 2, 3)
if lin_vel_y >= 0:
lin_vel_y *= np.abs(Y_RANGE[1])
else:
lin_vel_y *= np.abs(Y_RANGE[0])
if left_trigger < 0.1:
left_trigger = 0
if right_trigger < 0.1:
right_trigger = 0
if ang_vel >= 0:
ang_vel *= np.abs(YAW_RANGE[1])
else:
ang_vel *= np.abs(YAW_RANGE[0])
last_commands[0] = lin_vel_x
last_commands[1] = lin_vel_y
last_commands[2] = ang_vel
if not self.head_control_mode:
lin_vel_y = l_x
lin_vel_x = l_y
ang_vel = r_x
if lin_vel_x >= 0:
lin_vel_x *= np.abs(X_RANGE[1])
else:
last_commands[0] = 0.0
last_commands[1] = 0.0
last_commands[2] = 0.0
last_commands[3] = 0.0 # neck pitch 0 for now
lin_vel_x *= np.abs(X_RANGE[0])
head_yaw = l_x
head_pitch = l_y
head_roll = r_x
if lin_vel_y >= 0:
lin_vel_y *= np.abs(Y_RANGE[1])
else:
lin_vel_y *= np.abs(Y_RANGE[0])
if head_yaw >= 0:
head_yaw *= np.abs(HEAD_YAW_RANGE[0])
else:
head_yaw *= np.abs(HEAD_YAW_RANGE[1])
if ang_vel >= 0:
ang_vel *= np.abs(YAW_RANGE[1])
else:
ang_vel *= np.abs(YAW_RANGE[0])
if head_pitch >= 0:
head_pitch *= np.abs(HEAD_PITCH_RANGE[0])
else:
head_pitch *= np.abs(HEAD_PITCH_RANGE[1])
last_commands[0] = lin_vel_x
last_commands[1] = lin_vel_y
last_commands[2] = ang_vel
else:
last_commands[0] = 0.0
last_commands[1] = 0.0
last_commands[2] = 0.0
last_commands[3] = 0.0 # neck pitch 0 for now
if head_roll >= 0:
head_roll *= np.abs(HEAD_ROLL_RANGE[0])
else:
head_roll *= np.abs(HEAD_ROLL_RANGE[1])
head_yaw = l_x
head_pitch = l_y
head_roll = r_x
last_commands[4] = head_pitch
last_commands[5] = head_yaw
last_commands[6] = head_roll
if head_yaw >= 0:
head_yaw *= np.abs(HEAD_YAW_RANGE[0])
else:
head_yaw *= np.abs(HEAD_YAW_RANGE[1])
if head_pitch >= 0:
head_pitch *= np.abs(HEAD_PITCH_RANGE[0])
else:
head_pitch *= np.abs(HEAD_PITCH_RANGE[1])
if head_roll >= 0:
head_roll *= np.abs(HEAD_ROLL_RANGE[0])
else:
head_roll *= np.abs(HEAD_ROLL_RANGE[1])
last_commands[4] = head_pitch
last_commands[5] = head_yaw
last_commands[6] = head_roll
for event in pygame.event.get():
if self.p1.get_button(0): # A button
A_pressed = True
@ -145,5 +145,5 @@ if __name__ == "__main__":
controller = XBoxController(20)
while True:
controller.get_last_command()
print(controller.get_last_command())
time.sleep(0.05)

19
scripts/antennas_controller_test.py Normal file
View file

@ -0,0 +1,19 @@
from mini_bdx_runtime.xbox_controller import XBoxController
from mini_bdx_runtime.antennas import Antennas
import time
controller = XBoxController(60)
antennas = Antennas()
while True:
_, _, _, left_trigger, right_trigger = (
controller.get_last_command()
)
antennas.set_position_left(right_trigger)
antennas.set_position_right(left_trigger)
# print(left_trigger, right_trigger)
time.sleep(1/50)