From 7fd20b7ed47b1d8803c4cb4137efcaf280da3689 Mon Sep 17 00:00:00 2001
From: Zhiying Li <zhiyingli@g.ucla.edu>
Date: Thu, 11 Nov 2021 13:06:52 -0800
Subject: [PATCH] upload joystick code to alienware brance
---
.../Laptop_Code/joystick/controller_pwm.py | 139 +++++++++++++++++
.../Laptop_Code/joystick/controller_test.py | 147 ++++++++++++++++++
2 files changed, 286 insertions(+)
create mode 100644 Code/Control/Laptop_Code/joystick/controller_pwm.py
create mode 100644 Code/Control/Laptop_Code/joystick/controller_test.py
diff --git a/Code/Control/Laptop_Code/joystick/controller_pwm.py b/Code/Control/Laptop_Code/joystick/controller_pwm.py
new file mode 100644
index 0000000..f2f2e22
--- /dev/null
+++ b/Code/Control/Laptop_Code/joystick/controller_pwm.py
@@ -0,0 +1,139 @@
+import pygame
+
+def manual_vertical():
+ pwm1 = convertAxis(axes_win.get("vertical"))
+ pwm2 = pwm1
+ if pwm1 > 0:
+ dir1 = "+"
+ dir2 = "+"
+ else:
+ dir1 = "-"
+ dir2 = "-"
+
+ return abs(pwm1), abs(pwm2), dir1, dir2
+
+def manual_horizontal():
+ data_f = convertAxis(axes_win.get("forward"))
+ data_t = convertAxis(axes_win.get("turn"))
+ if abs(data_f) > abs(data_t):
+ pwm3 = data_f
+ pwm4 = pwm3
+ if pwm3 > 0:
+ dir3 = "+"
+ dir4 = "+"
+ else:
+ dir3 = "-"
+ dir4 = "-"
+ elif abs(data_f) < abs(data_t):
+ pwm3 = data_t
+ pwm4 = pwm3
+ if pwm3 > 0:
+ dir3 = "-"
+ dir4 = "+"
+ else:
+ dir3 = "+"
+ dir4 = "-"
+ else:
+ pwm3 = 0
+ pwm4 = 0
+ dir3 = "+"
+ dir4 = "-"
+
+ return abs(pwm3),abs(pwm4),dir3,dir4
+
+def manual_control():
+ pwm1,pwm2,dir1,dir2 = manual_vertical()
+ pwm3,pwm4,dir3,dir4 = manual_horizontal()
+ print("pwm1,pwm2,pwm3,pwm4:{},{},{},{}".format(pwm1,pwm2,pwm3,pwm4))
+ print("dir1,dir2,dir3,dir4:{},{},{},{}".format(dir1,dir2,dir3,dir4))
+ print("_________________________________")
+
+def rescale(oldValue, newMin=0, newMax=255):
+ oldMax = 1
+ oldMin = -1
+
+ oldRange = (oldMax - oldMin)
+ newRange = (newMax - newMin)
+ newValue = (((oldValue - oldMin) * newRange) / oldRange) + newMin
+ return int(newValue)
+
+def convertThrust(data):
+ return rescale(data, -255, 255)
+
+def convertAxis(axes):
+ data = joystick.get_axis(axes)*-1
+ pwm = convertThrust(data)
+ return pwm
+
+def go_back2_uper_level(flag_b,print_count_b):
+ if joystick.get_button(axes_win.get('button_back')):
+ flag_b = 0
+ print_count_b = 1
+ return flag_b,print_count_b
+
+def pygame_init(done):
+ for event in pygame.event.get():
+ if event.type == pygame.QUIT:
+ done = True
+ return done
+if __name__ == '__main__':
+ axes_win = dict(
+ forward = 1,
+ turn = 0,
+ vertical = 3,
+ button_a = 0, # auto control
+ button_b = 1, # manual_ball_capture
+ button_x = 2, # manual_no_ball_capture
+ button_y = 3, # let the lidar to determine ball caputure
+ button_LB = 4, # manual control
+ button_RB = 5, # stop all motors
+ button_back = 6, # go back to the first level
+ button_start = 7, # test camera core function
+ )
+ # print(axes_win.get('forward'))
+
+ # Set up joystick
+ pygame.init()
+ pygame.joystick.init()
+ done = False
+
+ flag = 0
+ print_count = 0
+
+ controller_id = 1
+ while not done:
+
+ done = pygame_init(done)
+
+ joystick = pygame.joystick.Joystick(controller_id)
+ joystick.init()
+
+ if joystick.get_button(axes_win.get('button_start')) and joystick.get_button(axes_win.get('button_back')):
+ print("kill the program")
+ done = True
+
+ elif joystick.get_button(axes_win.get('button_a')):
+ print("auto_control")
+
+ elif joystick.get_button(axes_win.get('button_RB')):
+ print("stop all motors")
+
+ elif joystick.get_button(axes_win.get('button_LB')):
+ print("manual control")
+ flag = 2
+ while (flag == 2):
+ done = pygame_init(done)
+ manual_control()
+ flag, print_count = go_back2_uper_level(flag, print_count)
+
+ elif joystick.get_button(axes_win.get('button_start')):
+ print("test the camera core function")
+
+ pygame.time.wait(100)
+
+ if print_count != 0:
+ print("No subsystem is running")
+ print_count = 0
+ pygame.time.wait(100)
+
+ pygame.quit()
\ No newline at end of file
diff --git a/Code/Control/Laptop_Code/joystick/controller_test.py b/Code/Control/Laptop_Code/joystick/controller_test.py
new file mode 100644
index 0000000..7d6eba4
--- /dev/null
+++ b/Code/Control/Laptop_Code/joystick/controller_test.py
@@ -0,0 +1,147 @@
+import pygame
+
+# Define some colors
+BLACK = (0, 0, 0)
+WHITE = (255, 255, 255)
+
+
+class TextPrint(object):
+ """
+ This is a simple class that will help us print to the screen
+ It has nothing to do with the joysticks, just outputting the
+ information.
+ """
+
+ def __init__(self):
+ """ Constructor """
+ self.reset()
+ self.x_pos = 10
+ self.y_pos = 10
+ self.font = pygame.font.Font(None, 20)
+
+ def print(self, my_screen, text_string):
+ """ Draw text onto the screen. """
+ text_bitmap = self.font.render(text_string, True, BLACK)
+ my_screen.blit(text_bitmap, [self.x_pos, self.y_pos])
+ self.y_pos += self.line_height
+
+ def reset(self):
+ """ Reset text to the top of the screen. """
+ self.x_pos = 10
+ self.y_pos = 10
+ self.line_height = 15
+
+ def indent(self):
+ """ Indent the next line of text """
+ self.x_pos += 10
+
+ def unindent(self):
+ """ Unindent the next line of text """
+ self.x_pos -= 10
+
+if __name__ == '__main__':
+
+ pygame.init()
+
+ # Set the width and height of the screen [width,height]
+ size = [500, 700]
+ screen = pygame.display.set_mode(size)
+
+ pygame.display.set_caption("My Game")
+
+ # Loop until the user clicks the close button.
+ done = False
+
+ # Used to manage how fast the screen updates
+ clock = pygame.time.Clock()
+
+ # Initialize the joysticks
+ pygame.joystick.init()
+
+ # Get ready to print
+ textPrint = TextPrint()
+
+ # -------- Main Program Loop -----------
+ while not done:
+ # EVENT PROCESSING STEP
+ for event in pygame.event.get():
+ if event.type == pygame.QUIT:
+ done = True
+
+ # Possible joystick actions: JOYAXISMOTION JOYBALLMOTION JOYBUTTONDOWN
+ # JOYBUTTONUP JOYHATMOTION
+ if event.type == pygame.JOYBUTTONDOWN:
+ print("Joystick button pressed.")
+ if event.type == pygame.JOYBUTTONUP:
+ print("Joystick button released.")
+
+ # DRAWING STEP
+ # First, clear the screen to white. Don't put other drawing commands
+ # above this, or they will be erased with this command.
+ screen.fill(WHITE)
+ textPrint.reset()
+
+ # Get count of joysticks
+ joystick_count = pygame.joystick.get_count()
+
+ textPrint.print(screen, "Number of joysticks: {}".format(joystick_count))
+ textPrint.indent()
+
+ # For each joystick:
+ for i in range(joystick_count):
+ joystick = pygame.joystick.Joystick(i)
+ joystick.init()
+
+ textPrint.print(screen, "Joystick {}".format(i))
+ textPrint.indent()
+
+ # Get the name from the OS for the controller/joystick
+ name = joystick.get_name()
+ textPrint.print(screen, "Joystick name: {}".format(name))
+
+ # Usually axis run in pairs, up/down for one, and left/right for
+ # the other.
+ axes = joystick.get_numaxes()
+ textPrint.print(screen, "Number of axes: {}".format(axes))
+ textPrint.indent()
+
+ for i in range(axes):
+ axis = joystick.get_axis(i)
+ print("{:>1.0f}".format(axis))
+ textPrint.print(screen, "Axis {} value: {:>6.3f}".format(i, axis))
+ textPrint.unindent()
+
+ buttons = joystick.get_numbuttons()
+ textPrint.print(screen, "Number of buttons: {}".format(buttons))
+ textPrint.indent()
+
+ for i in range(buttons):
+ button = joystick.get_button(i)
+ textPrint.print(screen, "Button {:>2} value: {}".format(i, button))
+ textPrint.unindent()
+
+ # Hat switch. All or nothing for direction, not like joysticks.
+ # Value comes back in an array.
+ hats = joystick.get_numhats()
+ textPrint.print(screen, "Number of hats: {}".format(hats))
+ textPrint.indent()
+
+ for i in range(hats):
+ hat = joystick.get_hat(i)
+ textPrint.print(screen, "Hat {} value: {}".format(i, str(hat)))
+ textPrint.unindent()
+
+ textPrint.unindent()
+
+ # ALL CODE TO DRAW SHOULD GO ABOVE THIS COMMENT
+
+ # Go ahead and update the screen with what we've drawn.
+ pygame.display.flip()
+
+ # Limit to 60 frames per second
+ clock.tick(60)
+
+ # Close the window and quit.
+ # If you forget this line, the program will 'hang'
+ # on exit if running from IDLE.
+ pygame.quit()
\ No newline at end of file
--
GitLab