Telos4
/
RoboRally
1
0
Fork 1
Code Issues Pull Requests Releases Wiki Activity

timer for game

simple_control
Simon Pirkelmann 2019-07-12 18:09:42 +02:00
parent a630150a24
commit 034d88394c
1 changed files with 220 additions and 132 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

352
remote_control/class_control_joystick.py
View File

@ -29,7 +29,6 @@ class race_track:
def __init__(self, robot_1, robot_2):
marker_sub = rospy.Subscriber("/marker_id_pos_angle", id_pos_angle, self.measurement_callback)
self.robot_1 = robot_1
self.robot_2 = robot_2
@ -55,8 +54,30 @@ class race_track:
self.checkpoint_count_2 = 0
self.finish_line_count_2 = 0
self.countdown = False
self.countdown_2 = False
self.time_start = False
self.countdown_check = False
self.robot_1_time = False
self.print_robot_1_time = False
self.print_robot_2_time = False
self.count_down_start = 0
self.time_circle_robot_1 = 0
self.time_circle_robot_2 = 0
self.print_GO = False
self.press_b_phrase = True
self.robot_1_second_circle = None
self.robot_1_first_circle = None
self.robot_2_second_circle = None
self.robot_2_first_circle = None
self.final_time = None
self.final_time_2 = None
def measurement_callback(self, data):
# Aruco marker number 14 it's robot 102; Aruco 1,5,7,8 are drawn small rectangle Polygon
# Aruco 2,4,0,11 are assembled big rectangle Polygon
@ -173,57 +194,186 @@ class race_track:
else:
print("couldn't find the all ARUCO markers")
def check_starting_position(self): # when you pressing B robot must staying on the start line for beginning of race
button_robot_1 = self.robot_1.joystick.get_button(1)
button_robot_2 = self.robot_2.joystick.get_button(1)
def check_starting_position(self, robot_1, robot_2): # when you pressing B robot must staying on the start line for beginning of race
button_robot_1 = robot_1.joystick.get_button(1)
button_robot_2 = robot_2.joystick.get_button(1)
if button_robot_1:
if self.start_strength_line.contains(self.robot_pos_1_point):
print("player 1 is on the start line")
print("MLS is on the start line")
self.robot_1.controlling_allowed_2 = True
self.countdown = True
else:
print("put the player 1 on the start line")
print("put the MLS on the start line")
self.robot_1.controlling_allowed_2 = False
if button_robot_2:
if self.start_strength_line.contains(self.robot_pos_2_point):
print("player 2 is on the start line")
print("BMW X6 is on the start line")
self.robot_2.controlling_allowed_2 = True
self.countdown_2 = True
else:
print("put the player 2 on the start line")
print("put the BMW X6 on the start line")
self.robot_2.controlling_allowed_2 = False
else:
print("no button pressed")
def start_countdown(self):
if self.countdown and self.countdown_2:
self.count_down_start = time.time()
self.time_start = True
self.countdown_check = True
self.press_b_phrase = False
self.countdown = False
def check_countdown(self):
# if countdown reaches 0 -> allow control
if self.countdown_check:
if time.time() - self.count_down_start < 5:
self.robot_1.controlling_allowed_2 = False
self.robot_2.controlling_allowed_2 = False
else:
self.print_GO = True
def print_countdown(self):
# prints countdown to screen
if self.time_start:
countdown_font = pygame.font.Font(None, 80)
ready_font= pygame.freetype.SysFont(None, 42)
ready_font.render_to(screen, (170, 200), "ARE YOU READY?", (255, 160, 30))
output_string = "{: 6.2f}".format(time.time() - self.count_down_start)
text = countdown_font.render(output_string, True, (0, 255, 0))
screen.blit(text, [260, 270])
def time_robot_1_2(self):
if self.robot_1_time and time.time() - self.count_down_start < 7.1:
self.time_circle_robot_1 = time.time()
self.time_circle_robot_2 = time.time()
self.print_robot_1_time = True
self.print_robot_2_time = True
def print_time_robot_1_2(self):
if self.print_robot_1_time:
time_robot_2_font_1 = pygame.freetype.SysFont(None, 42)
time_robot_2_font_1.render_to(screen, (30, 20), "TIME MLS", (255, 0, 0))
time_robot_1_font = pygame.font.Font(None, 52)
output_time_robot_1 = "{: 6.2f}".format(time.time() - self.time_circle_robot_1)
text = time_robot_1_font.render(output_time_robot_1, True, (255, 0, 0))
screen.blit(text, [120, 90])
if not self.final_time is None:
self.print_robot_1_time = False
if self.print_robot_2_time:
time_robot_2_font_2 = pygame.freetype.SysFont(None, 42)
time_robot_2_font_2.render_to(screen, (380, 20), "TIME BMW X6", (150, 50, 255))
time_robot_2_font = pygame.font.Font(None, 52)
output_time_robot_2 = "{: 6.2f}".format(time.time() - self.time_circle_robot_2)
text = time_robot_2_font.render(output_time_robot_2, True, (150, 50, 255))
screen.blit(text, [470, 90])
if not self.final_time_2 is None:
self.print_robot_2_time = False
def print_result(self):
if not self.final_time is None:
final_time_robot_1_font = pygame.font.Font(None, 36)
final_time_robot_1 = "FINAL TIME MLS {: 6.2f}".format(self.final_time)
text = final_time_robot_1_font.render(final_time_robot_1, True, (0, 255, 0))
screen.blit(text, [30, 300])
if not self.robot_1_second_circle is None:
second_circle_robot_1_font = pygame.font.Font(None, 32)
second_circle_robot_1 = "SECOND CIRCLE {: 6.2f}".format(self.robot_1_second_circle)
text = second_circle_robot_1_font.render(second_circle_robot_1, True, (255, 0, 0))
screen.blit(text, [30, 200])
if not self.robot_1_first_circle is None:
first_circle_robot_1_font = pygame.font.Font(None, 32)
first_circle_robot_1 = "FIRST CIRCLE {: 6.2f}".format(self.robot_1_first_circle)
text = first_circle_robot_1_font.render(first_circle_robot_1, True, (255, 0, 0))
screen.blit(text, [30, 150])
if not self.final_time_2 is None:
final_time_robot_2_font = pygame.font.Font(None, 36)
final_time_robot_2 = "FINAL TIME BMW X6 {: 6.2f}".format(self.final_time_2)
text = final_time_robot_2_font.render(final_time_robot_2, True, (0, 255, 0))
screen.blit(text, [380, 300])
if not self.robot_2_second_circle is None:
second_circle_robot_2_font = pygame.font.Font(None, 32)
second_circle_robot_2 = "SECOND CIRCLE {: 6.2f}".format(self.robot_2_second_circle)
text = second_circle_robot_2_font.render(second_circle_robot_2, True, (150, 50, 255))
screen.blit(text, [380, 200])
if not self.robot_2_first_circle is None:
first_circle_robot_2_font = pygame.font.Font(None, 32)
first_circle_robot_2 = "FIRST CIRCLE {: 6.2f}".format(self.robot_2_first_circle)
text = first_circle_robot_2_font.render(first_circle_robot_2, True, (150, 50, 255))
screen.blit(text, [380, 150])
def first_phrase(self):
if self.press_b_phrase:
robot_font_2 = pygame.freetype.SysFont(None, 30)
robot_font_2.render_to(screen, (240, 240), "BOTH PLAYERS!", (255, 160, 30))
robot_font_2.render_to(screen, (30, 300), "PUT DOWN TWO ROBOTS ON THE START LINE", (255, 160, 30))
robot_font_2.render_to(screen, (110, 360), "AND PRESS 'B' FOR THE BEGINNING", (255, 160, 30))
if self.print_GO:
if time.time() - self.count_down_start > 5 and time.time() - self.count_down_start < 7:
print_go_font = pygame.freetype.SysFont(None, 152)
print_go_font.render_to(screen, (230, 200), "GO!", (255, 160, 30))
self.time_start = False
else:
self.robot_1.controlling_allowed_2 = True
self.robot_2.controlling_allowed_2 = True
self.robot_1_time = True
def checkpoint(self):
if self.checkpoint_strength_line.contains(self.robot_pos_1_point):
if self.finish_line_count == self.checkpoint_count:
self.checkpoint_count += 1
print("Player 1 reached checkpoint, checkpoint count = {}".format(self.checkpoint_count))
if self.checkpoint_strength_line.contains(self.robot_pos_2_point):
if self.finish_line_count_2 == self.checkpoint_count_2:
self.checkpoint_count_2 += 1
print("Player 2 reached checkpoint, checkpoint count = {}".format(self.checkpoint_count_2))
if self.check_number_markers:
if self.checkpoint_strength_line.contains(self.robot_pos_1_point):
if self.finish_line_count == self.checkpoint_count:
self.checkpoint_count += 1
print("Player 1 reached checkpoint, checkpoint count = {}".format(self.checkpoint_count))
if self.checkpoint_strength_line.contains(self.robot_pos_2_point):
if self.finish_line_count_2 == self.checkpoint_count_2:
self.checkpoint_count_2 += 1
print("Player 2 reached checkpoint, checkpoint count = {}".format(self.checkpoint_count_2))
def finish_line(self):
if self.start_strength_line.contains(self.robot_pos_1_point):
if self.finish_line_count < self.checkpoint_count:
self.finish_line_count += 1
print("reached finish line, finish line count = {}".format(self.finish_line_count))
if self.check_number_markers:
if self.start_strength_line.contains(self.robot_pos_1_point):
if self.finish_line_count < self.checkpoint_count:
self.finish_line_count += 1
elif self.finish_line_count == self.checkpoint_count and self.finish_line_count == 3:
print("Player1 finished")
self.robot_1.controlling_allowed = False
elif self.finish_line_count == self.checkpoint_count and self.finish_line_count == 3:
if self.final_time is None:
self.final_time = time.time() - self.time_circle_robot_1
self.robot_1.controlling_allowed = False
if self.start_strength_line.contains(self.robot_pos_2_point):
if self.finish_line_count_2 < self.checkpoint_count_2:
self.finish_line_count_2 += 1
print("Player 2 reached finish line, finish line count = {}".format(self.finish_line_count_2))
elif self.finish_line_count == self.checkpoint_count and self.finish_line_count == 2:
if self.robot_1_second_circle is None:
self.robot_1_second_circle = time.time() - self.time_circle_robot_1
elif self.finish_line_count_2 == self.checkpoint_count_2 and self.finish_line_count_2 == 3:
self.robot_2.controlling_allowed = False
self.player_2 = False
if not self.player_2:
print("Player_2 win")
self.player_2 = True
elif self.finish_line_count == self.checkpoint_count and self.finish_line_count == 1:
if self.robot_1_first_circle is None:
self.robot_1_first_circle = time.time() - self.time_circle_robot_1
if self.start_strength_line.contains(self.robot_pos_2_point):
if self.finish_line_count_2 < self.checkpoint_count_2:
self.finish_line_count_2 += 1
print("Player 2 reached finish line, finish line count = {}".format(self.finish_line_count_2))
elif self.finish_line_count_2 == self.checkpoint_count_2 and self.finish_line_count_2 == 3:
if self.final_time_2 is None:
self.final_time_2 = time.time() - self.time_circle_robot_2
self.robot_2.controlling_allowed = False
self.player_2 = False
elif self.finish_line_count_2 == self.checkpoint_count_2 and self.finish_line_count_2 == 2:
if self.robot_2_second_circle is None:
self.robot_2_second_circle = time.time() - self.time_circle_robot_2
elif self.finish_line_count_2 == self.checkpoint_count_2 and self.finish_line_count_2 == 1:
if self.robot_2_first_circle is None:
self.robot_2_first_circle = time.time() - self.time_circle_robot_2
class robot: # we have 4 arg for this class, because joysticks get the same (value, axis) events
@ -238,7 +388,7 @@ class robot: # we have 4 arg for this class, because joysticks get the same (va
self.controlling_allowed = True
self.controlling_allowed_2 = False
self.joystick = pygame.joystick.Joystick(self.joy)
self.u1 = 0
self.u2 = 0
@ -271,86 +421,17 @@ class robot: # we have 4 arg for this class, because joysticks get the same (va
def check_control(self):
if not self.controlling_allowed:
if self.robot_stopped:# and self.robot0_stopped_2 and self.robot0_stopped_3 and self.robot0_stopped_4:
if self.robot_stopped:
pass
#print("check control -> already stopped")
else:
self.u1 = self.u2 = 0
self.robot_stopped = True
# self.robot0_stopped_2 = True
# self.robot0_stopped_3 = True
# self.robot0_stopped_4 = True
self.rc_socket.send('({},{})\n'.format(self.u1, self.u2).encode())
print("check control -> stop")
def control(self, event): # the control of two robots with joysticks
if self.controlling_allowed_2:
if self.controlling_allowed:
joy = event.joy
value = event.value*0.75
axis = event.axis
if joy == self.joy:
if axis == 1:
if abs(value) > 0.2:
u1 = u2 = -value
self.rc_socket.send('({},{})\n'.format(u1, u2).encode())
self.robot_stopped = False
elif not self.robot_stopped:
u1 = u2 = 0
self.rc_socket.send('({},{})\n'.format(u1, u2).encode())
self.robot_stopped = True
elif axis == 3:
if abs(value) > 0.2:
u1 = value
u2 = -value
self.rc_socket.send('({},{})\n'.format(u1, u2).encode())
self.robot0_stopped_2 = False
elif not self.robot0_stopped_2:
u1 = u2 = 0
self.rc_socket.send('({},{})\n'.format(u1, u2).encode())
self.robot0_stopped_2 = True
elif axis == 2:
if value > 0.2:
u1 = value/1.9
u2 = value/1.2
self.rc_socket.send('({},{})\n'.format(u1, u2).encode())
self.robot0_stopped_3 = False
elif not self.robot0_stopped_3:
u1 = u2 = 0
self.rc_socket.send('({},{})\n'.format(u1, u2).encode())
self.robot0_stopped_3 = True
elif axis == 5:
if value > 0.2:
u1 = value/1.2
u2 = value/1.9
self.rc_socket.send('({},{})\n'.format(u1, u2).encode())
self.robot0_stopped_4 = False
elif not self.robot0_stopped_4:
u1 = u2 = 0
self.rc_socket.send('({},{})\n'.format(u1, u2).encode())
self.robot0_stopped_4 = True
else: # not self.robot_stopped and not self.robot0_stopped_2
if self.robot_stopped and self.robot0_stopped_2 and self.robot0_stopped_3 and self.robot0_stopped_4:
pass
print("check control -> already stopped")
else:
u1 = u2 = 0
self.robot_stopped = True
self.robot0_stopped_2 = True
self.robot0_stopped_3 = True
self.robot0_stopped_4 = True
self.rc_socket.send('({},{})\n'.format(u1, u2).encode())
def control_alt(self):
if self.controlling_allowed and self.controlling_allowed_2:
ax1 = self.joystick.get_axis(1) * 0.75
@ -362,7 +443,7 @@ class robot: # we have 4 arg for this class, because joysticks get the same (va
u2_old = deepcopy(self.u2)
threshold = 0.2
if abs(ax1) < threshold and abs(ax2) < threshold and abs(ax3) < threshold and abs(ax5) < threshold and not self.robot_stopped:
if abs(ax1) < threshold and ax2 < threshold and abs(ax3) < threshold and ax5 < threshold and not self.robot_stopped:
self.robot_stopped = True
#print("all axis in neutral -> stopping")
self.u1 = self.u2 = 0
@ -379,7 +460,13 @@ class robot: # we have 4 arg for this class, because joysticks get the same (va
elif abs(ax3) > threshold:
self.u1 = ax3
self.u2 = -ax3
elif ax2 > threshold:
self.u1 = ax2
self.u2 = ax2*0.75
#self.rc_socket.send('({},{})\n'.format(u1, u2).encode())
elif ax5 > threshold:
self.u1 = ax5*0.75
self.u2 = ax5
if u1_old != self.u1 or u2_old != self.u2:
#print("update_control = True")
self.update_control = True
@ -407,13 +494,13 @@ class robot: # we have 4 arg for this class, because joysticks get the same (va
#self.rc_socket.send('({},{})\n'.format(u1, u2).encode())
elif pressed[pygame.K_UP]:
self.u1 = -1.0
self.u2 = -1.0
self.u1 = 1.0
self.u2 = 1.0
#self.rc_socket.send('({},{})\n'.format(u1, u2).encode())
elif pressed[pygame.K_DOWN]:
self.u1 = 1.0
self.u2 = 1.0
self.u1 = -1.0
self.u2 = -1.0
#self.rc_socket.send('({},{})\n'.format(u1, u2).encode())
elif event.type == pygame.KEYUP:
@ -441,13 +528,13 @@ class robot: # we have 4 arg for this class, because joysticks get the same (va
#self.rc_socket.send('({},{})\n'.format(u1, u2).encode())
elif pressed[pygame.K_w]:
self.u1 = -1.0
self.u2 = -1.0
self.u1 = 1.0
self.u2 = 1.0
#self.rc_socket.send('({},{})\n'.format(u1, u2).encode())
elif pressed[pygame.K_s]:
self.u1 = 1.0
self.u2 = 1.0
self.u1 = -1.0
self.u2 = -1.0
#self.rc_socket.send('({},{})\n'.format(u1, u2).encode())
elif event.type == pygame.KEYUP:
@ -461,20 +548,21 @@ class robot: # we have 4 arg for this class, because joysticks get the same (va
def main():
global screen
pygame.init()
screen= pygame.display.set_mode((640, 480))
robot_font = pygame.freetype.SysFont("shut up and play", 32)
robot_font_2 = pygame.freetype.SysFont("shut up and play", 22)
clock = pygame.time.Clock()
screen = pygame.display.set_mode((720, 576))
rospy.init_node('game_node', anonymous=True)
robot_1 = robot(0, '192.168.1.102', 1234)
robot_1.joystick_init()
robot_2 = robot(1, '192.168.1.101', 1234)
robot_2.joystick_init()
robot_track = race_track(robot_1, robot_2)
time.sleep(1.0)
while True:
robot_track.check_number_marker()
robot_track.polygon_rectangle()
@ -484,6 +572,9 @@ def main():
robot_1.check_control()
robot_2.check_control()
robot_track.check_countdown()
screen.fill((0, 0, 0))
events = pygame.event.get()
global pressed
@ -495,8 +586,8 @@ def main():
input_2_recorded = False
for event in events:
if event.type == pygame.JOYBUTTONDOWN:
robot_track.check_starting_position()
#robot_track.check_starting_position()
robot_track.check_starting_position(robot_1, robot_2)
robot_track.start_countdown()
if event.type == pygame.JOYAXISMOTION:
if event.joy == robot_2.joy:
robot_2.control_alt()
@ -515,16 +606,13 @@ def main():
robot_2.send_control()
if input_1_recorded:
robot_1.send_control()
screen.fill((0, 0, 0))
# You can use 'render' and then blit the text surface ...
# or just 'render_to' the target surface.
robot_font.render_to(screen, (80, 50), "Become your weapon and fight!", (255, 255, 255))
robot_font_2.render_to(screen, (280, 300), "PRESS 'B' FOR THE BEGINNING", (255, 130, 70))
robot_track.time_robot_1_2()
robot_track.print_countdown()
robot_track.print_time_robot_1_2()
robot_track.first_phrase()
robot_track.print_result()
pygame.display.flip()
if __name__ == '__main__':
main()
main()