├── README.md ├── moving-robot ├── car318_157.png ├── car400_200.png ├── car60_40.png ├── moving-robot-2.py ├── moving-robot.py ├── racetrack.jpg ├── racetrack2.jpg ├── racetrack2.png └── track.png ├── particle-filter ├── car60_40.png ├── particle-filter-2.py ├── particle-filter.gif └── particle-filter.py ├── planning ├── a-star.py ├── gridworld.py ├── gridworld.pyc ├── planning-2.py └── planning.py ├── slither ├── apple.png ├── eat.wav ├── eat2.wav ├── setup.py ├── slither.py └── snake.png └── tank-game ├── explosion.wav ├── gunshot1.wav ├── gunshot2.wav ├── shape_of_you_ed_sheeran.mp3 └── tank-game.py /README.md: -------------------------------------------------------------------------------- 1 | # pygame-robotics 2 | Basic robotics using pygame visualization. 3 | 4 | ![particle filter](particle-filter/particle-filter.gif) 5 | -------------------------------------------------------------------------------- /moving-robot/car318_157.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ioarun/pygame-robotics/5e966afaf69e843d3d96d21cedd0ee751b02cc76/moving-robot/car318_157.png -------------------------------------------------------------------------------- /moving-robot/car400_200.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ioarun/pygame-robotics/5e966afaf69e843d3d96d21cedd0ee751b02cc76/moving-robot/car400_200.png -------------------------------------------------------------------------------- /moving-robot/car60_40.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ioarun/pygame-robotics/5e966afaf69e843d3d96d21cedd0ee751b02cc76/moving-robot/car60_40.png -------------------------------------------------------------------------------- /moving-robot/moving-robot-2.py: -------------------------------------------------------------------------------- 1 | import pygame 2 | import random 3 | import time 4 | from math import * 5 | from copy import deepcopy 6 | 7 | display_width = 800 8 | display_height = 800 9 | 10 | world_size = display_width 11 | 12 | red = (200, 0, 0) 13 | blue = (0, 0, 255) 14 | green = (0, 155, 0) 15 | yellow = (200, 200, 0) 16 | white = (255, 255, 255) 17 | black = (0, 0, 0) 18 | grey = (67, 70, 75) 19 | 20 | car_length = 80.0 21 | car_width = 60.0 22 | 23 | wheel_length = 20 24 | wheel_width = 6 25 | 26 | max_steering_angle = pi/4 27 | 28 | car_img = pygame.image.load("car60_40.png") 29 | track_img= pygame.image.load("racetrack2.png") 30 | 31 | 32 | origin = (display_width/2, display_height/2) 33 | 34 | pygame.init() 35 | 36 | screen = pygame.display.set_mode((display_width, display_height)) 37 | pygame.display.set_caption("Moving robot") 38 | clock = pygame.time.Clock() 39 | 40 | screen.fill(white) 41 | 42 | class robot: 43 | def __init__(self): 44 | self.x = random.random()*world_size 45 | self.y = random.random()*world_size 46 | self.orientation = random.random() * 2.0 * pi 47 | self.steering_angle = 0.0 48 | self.steering_drift = 0.0 49 | self.forward_noise = 0.0 50 | self.turn_noise = 0.0 51 | self.sense_noise = 0.0 52 | 53 | def set(self, x, y, orientation, steering_angle): 54 | if x >= world_size or x < 0: 55 | raise ValueError, 'X coordinate out of bound' 56 | if y >= world_size or y < 0: 57 | raise ValueError, 'Y coordinate out of bound' 58 | if orientation >= 2*pi or orientation < 0: 59 | raise ValueError, 'Orientation must be in [0..2pi]' 60 | if abs(steering_angle) > max_steering_angle: 61 | raise ValueError, 'Exceeding max steering angle' 62 | 63 | self.x = x 64 | self.y = y 65 | self.orientation = orientation 66 | self.steering_angle = steering_angle 67 | 68 | def set_steering_drift(self, steering_drift): 69 | self.steering_drift = steering_drift 70 | 71 | 72 | def set_noise(self, f_noise, t_noise, s_noise): 73 | self.forward_noise = f_noise 74 | self.turn_noise = t_noise 75 | self.sense_noise = s_noise 76 | 77 | 78 | def move(self, turn, forward): 79 | theta = self.orientation # initial orientation 80 | alpha = turn # steering angle 81 | dist = forward # distance to be moved 82 | length = car_length # length of the robot 83 | if abs(alpha) > max_steering_angle: 84 | raise ValueError, 'Exceeding max steering angle' 85 | 86 | if dist < 0.0: 87 | raise ValueError, 'Moving backwards is not valid' 88 | 89 | # in local coordinates of robot 90 | beta = (dist/length)*tan(alpha) # turning angle 91 | # print degrees(beta) 92 | _x = _y = _theta = 0.0 93 | if beta > 0.001 or beta < -0.001: 94 | radius = dist/beta # turning radius 95 | cx = self.x - sin(theta)*radius # center of the circle 96 | cy = self.y - cos(theta)*radius # center of the circle 97 | 98 | # Uncomment to see the center of the circle the robot is going about 99 | # pygame.draw.circle(screen, red, (int(cx), int(cy)), 5) 100 | # pygame.display.update() 101 | 102 | # in global coordinates of robot 103 | _x = cx + sin(theta + beta)*radius 104 | _y = cy + cos(theta + beta)*radius 105 | _theta = (theta + beta)%(2*pi) 106 | 107 | else: # straight motion 108 | _x = self.x + dist*cos(theta) 109 | _y = self.y - dist*sin(theta) 110 | _theta = (theta + beta)%(2*pi) 111 | 112 | self.x = _x 113 | self.y = _y 114 | self.orientation = _theta 115 | self.steering_angle = alpha 116 | 117 | self.x %= world_size 118 | self.y %= world_size 119 | 120 | def sense(self, landmarks_loc, add_noise=False): 121 | Z = [] 122 | for i in range(len(landmarks_loc)): 123 | dist = sqrt((self.x - landmarks_loc[i][0])**2 + ((self.y - landmarks_loc[i][1])**2)) 124 | if add_noise: 125 | dist += random.gauss(0.0, self.sense_noise) 126 | 127 | Z.append(dist) 128 | 129 | return Z 130 | 131 | 132 | def cte(self, radius): 133 | cte = 0 134 | x, y, orientation = self.x, self.y, self.orientation 135 | if x <= 250: 136 | dx = x - 250 137 | dy = y - 400 138 | cte = sqrt(dx**2+dy**2) - 200 139 | elif 250 < x and x < 550: 140 | if (0 <= orientation and orientation < pi/2) or (3 * pi / 2 < orientation and orientation < 2 * pi): 141 | # print y 142 | cte = -(y - 200) 143 | else: 144 | cte = (y - 600) 145 | else: 146 | dx = x - 550 147 | dy = y - 400 148 | cte = sqrt(dx**2+dy**2) - 200 149 | return cte 150 | 151 | def pd(robot, diff_CTE): 152 | # p > d > i 153 | return 0.01*(robot.y - origin[1]) + 0.05*diff_CTE 154 | 155 | def draw_path(path, color): 156 | pygame.draw.lines(screen, color, False, [(i[1], i[0]) for i in path], 4) 157 | pygame.draw.line(screen, color, (path[0][1], path[0][0]),(path[len(path) - 1][1], path[len(path) - 1][0]), 4) 158 | 159 | def smoother(path, fix, weight_data = 0.2, weight_smooth = 0.2, tolerance = 0.00001): 160 | 161 | change = tolerance 162 | newpath = deepcopy(path) 163 | 164 | while change >= tolerance: 165 | change = 0 166 | for i in range(len(path)): 167 | for j in range(len(path[0])): 168 | 169 | newpath[i][j] += weight_smooth*(newpath[(i-1)%len(path)][j] + newpath[(i+1)%len(path)][j] - 2.0*newpath[i][j]) + \ 170 | (weight_smooth / 2.0)*(2.0*newpath[(i-1)%len(path)][j] - newpath[(i-2)%len(path)][j] - newpath[i][j]) +\ 171 | (weight_smooth / 2.0)*(2.0*newpath[(i+1)%len(path)][j] - newpath[(i+2)%len(path)][j] - newpath[i][j]) 172 | 173 | return newpath 174 | 175 | 176 | def draw_rect(center, corners, rotation_angle, color): 177 | c_x = center[0] 178 | c_y = center[1] 179 | delta_angle = rotation_angle 180 | rotated_corners = [] 181 | 182 | for p in corners: 183 | temp = [] 184 | length = sqrt((p[0] - c_x)**2 + (c_y - p[1])**2) 185 | angle = atan2(c_y - p[1], p[0] - c_x) 186 | angle += delta_angle 187 | temp.append(c_x + length*cos(angle)) 188 | temp.append(c_y - length*sin(angle)) 189 | rotated_corners.append(temp) 190 | 191 | # draw rectangular polygon --> car body 192 | rect = pygame.draw.polygon(screen, color, (rotated_corners[0],rotated_corners[1],rotated_corners[2],rotated_corners[3])) 193 | 194 | def draw_track(cx,cy,radius,color): 195 | # pygame.draw.line(screen, color, (cx-radius, cy-radius), (cx-radius, cy+radius), 200) 196 | # pygame.draw.line(screen, color, (cx-radius, cy-radius), (3*cx, cy-radius), 200) 197 | # pygame.draw.line(screen, color, (3*cx, cy-radius), (3*cx, cy+radius), 200) 198 | # pygame.draw.line(screen, color, (cx-radius, cy+radius), (3*cx, cy+radius), 200) 199 | 200 | pygame.draw.arc(screen, white, (cx-radius,cy-radius,2*radius,2*radius), radians(90), radians(270), 5) 201 | pygame.draw.line(screen, white, (cx, cy-radius), (cx+300, cy-radius), 5) 202 | pygame.draw.arc(screen, white, (cx-radius+300,cy-radius,2*radius,2*radius), -pi/2,pi/2, 5) 203 | pygame.draw.line(screen, white, (cx, cy+radius), (cx+300, cy+radius), 5) 204 | 205 | 206 | 207 | def draw_robot(robot): 208 | car_x = robot.x 209 | car_y = robot.y 210 | orientation = robot.orientation 211 | steering_angle = robot.steering_angle 212 | 213 | p1 = [car_x-car_length/4,car_y-car_width/2] 214 | p2 = [car_x+(0.75*car_length),car_y-car_width/2] 215 | p3 = [car_x+(0.75*car_length),car_y+car_width/2] 216 | p4 = [car_x-car_length/4,car_y+car_width/2] 217 | 218 | # car body 219 | draw_rect([car_x, car_y], [p1, p2, p3, p4], orientation, yellow) 220 | 221 | # heading direction 222 | # h = [car_x+car_length/2,car_y] 223 | # length = car_length/2 224 | # angle = atan2(car_y - h[1], h[0] - car_x) 225 | # angle += orientation 226 | # h[0] = car_x + length*cos(angle) 227 | # h[1] = car_y - length*sin(angle) 228 | 229 | # wheels 230 | # rotate center of wheel1 231 | w1_c_x = car_x 232 | w1_c_y = car_y - car_width/3 233 | length = sqrt((w1_c_x - car_x)**2 + (car_y - w1_c_y)**2) 234 | angle = atan2(car_y - w1_c_y, w1_c_x - car_x) 235 | angle += orientation 236 | w1_c_x = car_x + length*cos(angle) 237 | w1_c_y = car_y - length*sin(angle) 238 | 239 | # draw corners of wheel1 240 | w1_p1 = [w1_c_x-wheel_length/2, w1_c_y-wheel_width/2] 241 | w1_p2 = [w1_c_x+wheel_length/2, w1_c_y-wheel_width/2] 242 | w1_p3 = [w1_c_x+wheel_length/2, w1_c_y+wheel_width/2] 243 | w1_p4 = [w1_c_x-wheel_length/2, w1_c_y+wheel_width/2] 244 | draw_rect([w1_c_x, w1_c_y], [w1_p1, w1_p2, w1_p3, w1_p4], orientation, black) 245 | 246 | 247 | 248 | 249 | 250 | w2_c_x = car_x + car_length/2 251 | w2_c_y = car_y - car_width/3 252 | length = sqrt((w2_c_x - car_x)**2 + (car_y - w2_c_y)**2) 253 | angle = atan2(car_y - w2_c_y, w2_c_x - car_x) 254 | angle += orientation 255 | w2_c_x = car_x + length*cos(angle) 256 | w2_c_y = car_y - length*sin(angle) 257 | 258 | w2_p1 = [w2_c_x-wheel_length/2, w2_c_y-wheel_width/2] 259 | w2_p2 = [w2_c_x+wheel_length/2, w2_c_y-wheel_width/2] 260 | w2_p3 = [w2_c_x+wheel_length/2, w2_c_y+wheel_width/2] 261 | w2_p4 = [w2_c_x-wheel_length/2, w2_c_y+wheel_width/2] 262 | draw_rect([w2_c_x, w2_c_y], [w2_p1, w2_p2, w2_p3, w2_p4], steering_angle + orientation, black) 263 | # rect = pygame.draw.polygon(screen, black, (w2_p1,w2_p2,w2_p3,w2_p4)) 264 | 265 | 266 | w3_c_x = car_x + car_length/2 267 | w3_c_y = car_y + car_width/3 268 | length = sqrt((w3_c_x - car_x)**2 + (car_y - w3_c_y)**2) 269 | angle = atan2(car_y - w3_c_y, w3_c_x - car_x) 270 | angle += orientation 271 | w3_c_x = car_x + length*cos(angle) 272 | w3_c_y = car_y - length*sin(angle) 273 | 274 | w3_p1 = [w3_c_x-wheel_length/2, w3_c_y-wheel_width/2] 275 | w3_p2 = [w3_c_x+wheel_length/2, w3_c_y-wheel_width/2] 276 | w3_p3 = [w3_c_x+wheel_length/2, w3_c_y+wheel_width/2] 277 | w3_p4 = [w3_c_x-wheel_length/2, w3_c_y+wheel_width/2] 278 | draw_rect([w3_c_x, w3_c_y], [w3_p1, w3_p2, w3_p3, w3_p4], steering_angle + orientation, black) 279 | # rect = pygame.draw.polygon(screen, black, (w3_p1,w3_p2,w3_p3,w3_p4)) 280 | 281 | 282 | 283 | w4_c_x = car_x 284 | w4_c_y = car_y + car_width/3 285 | length = sqrt((w4_c_x - car_x)**2 + (car_y - w4_c_y)**2) 286 | angle = atan2(car_y - w4_c_y, w4_c_x - car_x) 287 | angle += orientation 288 | w4_c_x = car_x + length*cos(angle) 289 | w4_c_y = car_y - length*sin(angle) 290 | 291 | w4_p1 = [w4_c_x-wheel_length/2, w4_c_y-wheel_width/2] 292 | w4_p2 = [w4_c_x+wheel_length/2, w4_c_y-wheel_width/2] 293 | w4_p3 = [w4_c_x+wheel_length/2, w4_c_y+wheel_width/2] 294 | w4_p4 = [w4_c_x-wheel_length/2, w4_c_y+wheel_width/2] 295 | draw_rect([w4_c_x, w4_c_y], [w4_p1, w4_p2, w4_p3, w4_p4], orientation, black) 296 | 297 | # pygame.draw.line(screen, red, (h[0], h[1]),(int(car_x), int(car_y)), 1) 298 | 299 | # draw axle 300 | pygame.draw.line(screen, black, (w1_c_x, w1_c_y),(w4_c_x, w4_c_y), 1) 301 | 302 | # draw mid of axle 303 | pygame.draw.circle(screen, red, (int(car_x), int(car_y)), 3) 304 | 305 | 306 | # col, row 307 | # path = [[200, 200],[200, 250],[200, 300], [200, 350],[200, 400],[200, 450],[200, 500],[200, 550],[200, 600],\ 308 | # [250, 600], [300, 600], [350, 600],[400, 600],[450, 600], [500, 600], [550, 600], [600, 600],[600, 550], [600, 500],[600, 450],[600, 400],\ 309 | # [600, 350],[600, 300],[600, 250],[600, 200], [550, 200],[500, 200],[450, 200],[400, 200], [350, 200],[300, 200],[250, 200]] 310 | 311 | # fixed_pts = [0 for _ in range(len(path))] 312 | # fixed_pts[0] = 1 313 | # fixed_pts[4] = 1 314 | # fixed_pts[8] = 1 315 | # fixed_pts[12]= 1 316 | 317 | robot = robot() 318 | # robot.set_noise(0.1, 0.01, 5.0) 319 | 320 | orientation = 90.0 321 | steering_angle = 0.0 322 | #in radians 323 | robot.set(50, 400,radians(orientation), steering_angle) 324 | 325 | exit = False 326 | 327 | delta_forward = 0.0 328 | delta_steer = 0.0 329 | 330 | previous_CTE = robot.cte(200) 331 | CTE = robot.cte(200) 332 | int_crosstrack_error = 0.0 333 | crosstrack_error = robot.cte(200) 334 | while exit == False: 335 | 336 | screen.fill(white) 337 | screen.blit(track_img, (0, 100)) 338 | # Uncomment following to see the exact racetrack 339 | #draw_track(250, 400, 200, grey) 340 | # draw_path(path, red) 341 | 342 | draw_robot(robot) 343 | 344 | # pygame.draw.line(screen, green, (display_width/2, 0), (display_width/2, display_height), 1) 345 | # pygame.draw.line(screen, black, (0, display_height/2), (display_width, display_height/2), 1) 346 | 347 | pygame.display.update() 348 | clock.tick(100) 349 | 350 | for event in pygame.event.get(): 351 | if event.type == pygame.QUIT: 352 | exit = True 353 | if event.type == pygame.KEYDOWN: 354 | if event.key == pygame.K_LEFT: 355 | delta_steer = radians(1) 356 | elif event.key == pygame.K_RIGHT: 357 | delta_steer = -radians(1) 358 | elif event.key == pygame.K_UP: 359 | delta_forward = 5.0 360 | elif event.key == pygame.K_DOWN: 361 | delta_forward = -5.0 362 | elif event.type == pygame.KEYUP: 363 | if event.key == pygame.K_RIGHT or event.key == pygame.K_LEFT: 364 | delta_steer = 0.0 365 | if event.key == pygame.K_UP or event.key == pygame.K_DOWN: 366 | delta_forward = 0.0 367 | 368 | diff_crosstrack_error = -crosstrack_error 369 | crosstrack_error = robot.cte(200) 370 | diff_crosstrack_error += crosstrack_error 371 | int_crosstrack_error += crosstrack_error 372 | steering_angle = - 0.1 * crosstrack_error \ 373 | - 0.5 * diff_crosstrack_error 374 | if steering_angle > max_steering_angle: 375 | steering_angle = max_steering_angle 376 | elif steering_angle < -max_steering_angle: 377 | steering_angle = -max_steering_angle 378 | print crosstrack_error 379 | robot.move(steering_angle, 2) 380 | -------------------------------------------------------------------------------- /moving-robot/moving-robot.py: -------------------------------------------------------------------------------- 1 | import pygame 2 | import random 3 | import time 4 | from math import * 5 | 6 | display_width = 800 7 | display_height = 800 8 | 9 | world_size = display_width 10 | 11 | red = (200, 0, 0) 12 | blue = (0, 0, 255) 13 | green = (0, 155, 0) 14 | yellow = (155, 155, 0) 15 | white = (255, 255, 255) 16 | black = (0, 0, 0) 17 | 18 | car_length = 400 19 | car_width = 200 20 | 21 | car_img = pygame.image.load("car60_40.png") 22 | 23 | origin = (display_width/2, display_height/2) 24 | 25 | pygame.init() 26 | 27 | screen = pygame.display.set_mode((display_width, display_height)) 28 | pygame.display.set_caption("Moving robot") 29 | clock = pygame.time.Clock() 30 | 31 | screen.fill(white) 32 | 33 | class robot: 34 | def __init__(self): 35 | self.x = random.random()*world_size 36 | self.y = random.random()*world_size 37 | self.orientation = random.random() * 2.0 * pi 38 | self.forward_noise = 0.0 39 | self.turn_noise = 0.0 40 | self.sense_noise = 0.0 41 | 42 | def set(self, x, y, orientation): 43 | if x >= world_size or x < 0: 44 | raise ValueError, 'X coordinate out of bound' 45 | if y >= world_size or y < 0: 46 | raise ValueError, 'Y coordinate out of bound' 47 | if orientation >= 2*pi or orientation < 0: 48 | raise ValueError, 'Orientation must be in [0..2pi]' 49 | 50 | self.x = x 51 | self.y = y 52 | self.orientation = orientation 53 | 54 | 55 | def set_noise(self, f_noise, t_noise, s_noise): 56 | self.forward_noise = f_noise 57 | self.turn_noise = t_noise 58 | self.sense_noise = s_noise 59 | 60 | 61 | def move(self, turn, forward): 62 | if forward < 0: 63 | raise ValueError, 'Cant move backwards' 64 | 65 | self.orientation = self.orientation + turn + random.gauss(0.0, self.turn_noise) 66 | self.orientation %= 2*pi 67 | 68 | dist = forward + random.gauss(0.0, self.forward_noise) 69 | self.x = self.x + dist*cos(self.orientation) 70 | self.y = self.y - dist*sin(self.orientation) 71 | 72 | self.x %= world_size 73 | self.y %= world_size 74 | 75 | def sense(self, landmarks_loc, add_noise=False): 76 | Z = [] 77 | for i in range(len(landmarks_loc)): 78 | dist = sqrt((self.x - landmarks_loc[i][0])**2 + ((self.y - landmarks_loc[i][1])**2)) 79 | if add_noise: 80 | dist += random.gauss(0.0, self.sense_noise) 81 | 82 | Z.append(dist) 83 | 84 | return Z 85 | 86 | 87 | def draw_robot(robot): 88 | car_x = robot.x - 30 89 | car_y = robot.y - 20 90 | orientation = robot.orientation 91 | img = pygame.transform.rotate(car_img, 0) 92 | img = pygame.transform.rotate(car_img, orientation*180/pi) 93 | screen.blit(img, (car_x, car_y)) 94 | 95 | # pygame.draw.rect(screen, yellow, (car_x, car_y, 400, 200)) 96 | 97 | # draw wheels 98 | # wheel_l = 100 99 | # wheel_w = 20 100 | # pygame.draw.rect(screen, black, (car_x - wheel_l, robot.y, wheel_l, wheel_w)) 101 | # # in radians 102 | # print orientation 103 | # # in degrees 104 | # print orientation*180/pi 105 | # pygame.draw.circle(screen, blue, (int(robot.x), int(robot.y)), 5) 106 | # rect = pygame.draw.polygon(screen, blue, ((car_x-car_length/2,car_y-car_width/2),(car_x+car_length/2,car_y-car_width/2), \ 107 | # (car_x + car_length/2, car_y + car_width/2), (car_x-car_length/2, car_y+car_width/2))) 108 | 109 | landmarks_loc = [[200, 200], [600, 600], [200, 600], [600, 200]] 110 | 111 | robot = robot() 112 | # robot.set_noise(0.1, 0.01, 5.0) 113 | 114 | orientation = 0 115 | #in radians 116 | orientation = orientation*pi/180 117 | robot.set(origin[0], origin[1], orientation) 118 | 119 | exit = False 120 | 121 | delta_orient = 0.0 122 | delta_forward = 0.0 123 | 124 | while exit == False: 125 | 126 | screen.fill(white) 127 | pygame.draw.line(screen, green, (display_width/2, 0), (display_width/2, display_height), 1) 128 | pygame.draw.line(screen, black, (0, display_height/2), (display_width, display_height/2), 1) 129 | for i in range(len(landmarks_loc)): 130 | pygame.draw.circle(screen, blue, landmarks_loc[i], 20) 131 | 132 | draw_robot(robot) 133 | pygame.display.update() 134 | clock.tick(60) 135 | 136 | for event in pygame.event.get(): 137 | if event.type == pygame.QUIT: 138 | exit = True 139 | if event.type == pygame.KEYDOWN: 140 | if event.key == pygame.K_LEFT: 141 | delta_orient = 0.0175 142 | elif event.key == pygame.K_RIGHT: 143 | delta_orient = -0.0175 144 | elif event.key == pygame.K_UP: 145 | delta_forward = 1 146 | elif event.key == pygame.K_DOWN: 147 | delta_forward = -1 148 | elif event.type == pygame.KEYUP: 149 | if event.key == pygame.K_RIGHT or event.key == pygame.K_LEFT or event.key == pygame.K_UP \ 150 | or event.key == pygame.K_DOWN: 151 | delta_orient = 0.0 152 | delta_forward = 0.0 153 | 154 | robot.move(delta_orient, delta_forward) 155 | print robot.sense(landmarks_loc) 156 | -------------------------------------------------------------------------------- /moving-robot/racetrack.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ioarun/pygame-robotics/5e966afaf69e843d3d96d21cedd0ee751b02cc76/moving-robot/racetrack.jpg -------------------------------------------------------------------------------- /moving-robot/racetrack2.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ioarun/pygame-robotics/5e966afaf69e843d3d96d21cedd0ee751b02cc76/moving-robot/racetrack2.jpg -------------------------------------------------------------------------------- /moving-robot/racetrack2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ioarun/pygame-robotics/5e966afaf69e843d3d96d21cedd0ee751b02cc76/moving-robot/racetrack2.png -------------------------------------------------------------------------------- /moving-robot/track.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ioarun/pygame-robotics/5e966afaf69e843d3d96d21cedd0ee751b02cc76/moving-robot/track.png -------------------------------------------------------------------------------- /particle-filter/car60_40.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ioarun/pygame-robotics/5e966afaf69e843d3d96d21cedd0ee751b02cc76/particle-filter/car60_40.png -------------------------------------------------------------------------------- /particle-filter/particle-filter-2.py: -------------------------------------------------------------------------------- 1 | import pygame 2 | import random 3 | import time 4 | from math import * 5 | from copy import deepcopy 6 | 7 | display_width = 800 8 | display_height = 800 9 | 10 | world_size = display_width 11 | 12 | red = (200, 0, 0) 13 | blue = (0, 0, 255) 14 | green = (0, 155, 0) 15 | yellow = (255, 255, 0) 16 | white = (255, 255, 255) 17 | black = (0, 0, 0) 18 | 19 | car_length = 60 20 | car_width = 40 21 | 22 | car_img = pygame.image.load("car60_40.png") 23 | 24 | origin = (display_width/2, display_height/2) 25 | 26 | pygame.init() 27 | 28 | screen = pygame.display.set_mode((display_width, display_height)) 29 | pygame.display.set_caption("Moving robot") 30 | clock = pygame.time.Clock() 31 | 32 | screen.fill(white) 33 | 34 | class robot: 35 | def __init__(self): 36 | self.x = random.random()*world_size 37 | self.y = random.random()*world_size 38 | self.orientation = random.random() * 2.0 * pi 39 | self.forward_noise = 0.0 40 | self.turn_noise = 0.0 41 | self.sense_noise = 0.0 # or bearing_noise 42 | 43 | def set(self, x, y, orientation): 44 | if x >= world_size or x < 0: 45 | raise ValueError, 'X coordinate out of bound' 46 | if y >= world_size or y < 0: 47 | raise ValueError, 'Y coordinate out of bound' 48 | if orientation >= 2*pi or orientation < 0: 49 | raise ValueError, 'Orientation must be in [0..2pi]' 50 | 51 | self.x = x 52 | self.y = y 53 | self.orientation = orientation 54 | 55 | 56 | def set_noise(self, f_noise, t_noise, s_noise): 57 | self.forward_noise = f_noise 58 | self.turn_noise = t_noise 59 | self.sense_noise = s_noise 60 | 61 | 62 | def move(self, turn, forward): 63 | if forward < 0: 64 | raise ValueError, 'Cant move backwards' 65 | 66 | self.orientation = self.orientation + turn + random.gauss(0.0, self.turn_noise) 67 | self.orientation %= 2*pi 68 | 69 | dist = forward + random.gauss(0.0, self.forward_noise) 70 | self.x = self.x + dist*cos(self.orientation) 71 | self.y = self.y - dist*sin(self.orientation) 72 | 73 | self.x %= world_size 74 | self.y %= world_size 75 | 76 | temp = robot() 77 | temp.set_noise(0.001, 0.1, 0.1) 78 | temp.set(self.x, self.y, self.orientation) 79 | temp.set_noise(self.forward_noise, self.turn_noise, self.sense_noise) 80 | return temp 81 | 82 | def sense(self, landmarks, add_noise=False): 83 | Z = [] 84 | for i in range(len(landmarks)): 85 | bearing_angle = atan2(landmarks[i][0] - self.y, landmarks[i][1] - self.x) - self.orientation 86 | 87 | if add_noise: 88 | bearing_angle += random.gauss(0.0, self.bearing_noise) 89 | 90 | # avoid angles greater than 2pi 91 | bearing_angle %= 2*pi 92 | Z.append(bearing_angle) 93 | 94 | return Z # Return vector Z of 4 bearings. 95 | 96 | def measurement_prob(self, measurements, landmarks): 97 | # calculate the correct measurement 98 | predicted_measurements = self.sense(landmarks) 99 | 100 | # compute errors 101 | error = 1.0 102 | for i in range(len(measurements)): 103 | error_bearing = abs(measurements[i] - predicted_measurements[i]) 104 | error_bearing = (error_bearing + pi) % (2.0 * pi) - pi # truncate 105 | 106 | # update Gaussian 107 | error *= (exp(- (error_bearing ** 2) / (self.sense_noise ** 2) / 2.0) / 108 | sqrt(2.0 * pi * (self.sense_noise ** 2))) 109 | 110 | return error 111 | 112 | 113 | def Gaussian(self, mu, sigma, x): 114 | 115 | # calculates the probability of x for 1-dim Gaussian with mean mu and var. sigma 116 | return exp(- ((mu - x) ** 2) / (sigma ** 2) / 2.0) / sqrt(2.0 * pi * (sigma ** 2)) 117 | 118 | def draw_robot(car): 119 | x = car.x 120 | y = car.y 121 | orientation = car.orientation 122 | img = pygame.transform.rotate(car_img, orientation*180/pi) 123 | screen.blit(img, (x-30, y-20)) 124 | # # in radians 125 | # print orientation 126 | # # in degrees 127 | # print orientation*180/pi 128 | # pygame.draw.circle(screen, blue, (int(x), int(y)), 5) 129 | # rect = pygame.draw.polygon(screen, blue, ((x-car_length/2,y-car_width/2),(x+car_length/2,y-car_width/2), \ 130 | # (x + car_length/2, y + car_width/2), (x-car_length/2, y+car_width/2))) 131 | 132 | def draw_particles(particles): 133 | for i in range(len(particles)): 134 | x = particles[i].x 135 | y = particles[i].y 136 | orientation = particles[i].orientation 137 | pygame.draw.circle(screen, green, (int(x), int(y)), 5) 138 | 139 | 140 | landmarks_loc = [[200, 200], [600, 600], [200, 600], [600, 200], [200, 300], [300, 200], [500, 200],\ 141 | [200, 200], [200, 500], [300, 600],[500, 600], [600, 500], [600, 300], [400, 200],\ 142 | [200, 400], [400, 600], [600, 400]] 143 | 144 | car = robot() 145 | # car.set_noise(0.1, 0.1, 0.1) 146 | 147 | orientation = 0 148 | #in radians 149 | orientation = orientation*pi/180 150 | car.set(origin[0], origin[1], orientation) 151 | 152 | exit = False 153 | 154 | delta_orient = 0.0 155 | delta_forward = 0.0 156 | 157 | particles = [] 158 | # create particles 159 | for i in range(1000): 160 | particle = robot() 161 | # particle.orientation = orientation 162 | particle.set_noise(0.001, 0.1, 0.1) 163 | particles.append(particle) 164 | 165 | while exit == False: 166 | 167 | screen.fill(white) 168 | pygame.draw.line(screen, green, (display_width/2, 0), (display_width/2, display_height), 1) 169 | pygame.draw.line(screen, black, (0, display_height/2), (display_width, display_height/2), 1) 170 | for i in range(len(landmarks_loc)): 171 | pygame.draw.circle(screen, blue, landmarks_loc[i], 20) 172 | 173 | draw_robot(car) 174 | draw_particles(particles) 175 | 176 | pygame.display.update() 177 | clock.tick(100) 178 | 179 | for event in pygame.event.get(): 180 | if event.type == pygame.QUIT: 181 | exit = True 182 | if event.type == pygame.KEYDOWN: 183 | if event.key == pygame.K_LEFT: 184 | delta_orient = 0.0175 185 | elif event.key == pygame.K_RIGHT: 186 | delta_orient = -0.0175 187 | elif event.key == pygame.K_UP: 188 | delta_forward = 2 189 | elif event.key == pygame.K_DOWN: 190 | delta_forward = -2 191 | elif event.type == pygame.KEYUP: 192 | if event.key == pygame.K_RIGHT or event.key == pygame.K_LEFT or event.key == pygame.K_UP \ 193 | or event.key == pygame.K_DOWN: 194 | delta_orient = 0.0 195 | delta_forward = 0.0 196 | 197 | car.move(delta_orient, delta_forward) 198 | particles2 = [] 199 | for particle in particles: 200 | # print "before :",particle.x, particle.y, particle.orientation 201 | # particle.orientation = car.orientation 202 | particles2.append(particle.move(delta_orient, delta_forward)) 203 | # print "afer :",particle.x, particle.y, particle.orientation 204 | 205 | particles = particles2 206 | 207 | measurements = car.sense(landmarks_loc) 208 | 209 | weights = [] 210 | for i in range(1000): 211 | weights.append(particles[i].measurement_prob(measurements, landmarks_loc)) 212 | 213 | # resampling 214 | p = [] 215 | index = int(random.random() * 1000) 216 | beta = 0.0 217 | mw = max(weights) 218 | for i in range(1000): 219 | beta += random.random() * 2.0 * mw 220 | while beta > weights[index]: 221 | beta -= weights[index] 222 | index = (index + 1) % 1000 223 | p.append(particles[index]) 224 | particles = deepcopy(p) -------------------------------------------------------------------------------- /particle-filter/particle-filter.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ioarun/pygame-robotics/5e966afaf69e843d3d96d21cedd0ee751b02cc76/particle-filter/particle-filter.gif -------------------------------------------------------------------------------- /particle-filter/particle-filter.py: -------------------------------------------------------------------------------- 1 | import pygame 2 | import random 3 | from math import * 4 | import time 5 | 6 | 7 | display_width = 500 8 | display_height = 500 9 | 10 | world_size = min(display_width - 10, display_height - 10) 11 | 12 | red = (200, 0, 0) 13 | blue = (0, 0, 255) 14 | green = (0, 155, 0) 15 | yellow = (255, 255, 0) 16 | white = (255, 255, 255) 17 | black = (0, 0, 0) 18 | 19 | car_length = 40 20 | car_width = 20 21 | 22 | pygame.init() 23 | 24 | screen = pygame.display.set_mode((display_width, display_height)) 25 | pygame.display.set_caption("Particle Filter Demo") 26 | clock = pygame.time.Clock() 27 | 28 | screen.fill(white) 29 | 30 | def car(x, y): 31 | pygame.draw.rect(screen, blue, (x, y, car_length, car_width)) 32 | 33 | def landmarks(locations): 34 | for loc in locations: 35 | pygame.draw.circle(screen, black, loc, 20) 36 | 37 | def sense(car_x, car_y, landmarks_loc, noise=False): 38 | measurements = [] 39 | for i in range(len(landmarks_loc)): 40 | dist = sqrt((car_x - landmarks_loc[i][0])**2 + (car_y - landmarks_loc[i][1])**2) 41 | 42 | if noise: 43 | ## add gaussian noise to the sensed distance 44 | dist += random.gauss(0.0, 5.0) 45 | 46 | measurements.append(dist) 47 | 48 | return measurements 49 | 50 | def measurement_prob(x, y, landmarks_loc, measurements): 51 | predicted_measurements = sense(x, y, landmarks_loc, noise=False) 52 | 53 | prob = 1.0 54 | for i in range(len(landmarks_loc)): 55 | prob *= Gaussian(predicted_measurements[i], 5.0, measurements[i]) 56 | 57 | return prob 58 | 59 | 60 | def Gaussian(mu, sigma, x): 61 | return exp(- ((mu - x) ** 2) / (sigma ** 2) / 2.0) / sqrt(2.0 * pi * (sigma ** 2)) 62 | 63 | 64 | def repeat(car_x, car_y, landmarks_loc, particles, particle_size): 65 | time.sleep(1) 66 | screen.fill(white) 67 | landmarks(landmarks_loc) 68 | 69 | # move the car 70 | car_x += 10 71 | car_y -= 10 72 | 73 | car_x %= world_size 74 | car_y %= world_size 75 | 76 | car(car_x, car_y) 77 | 78 | measurements = sense(car_x, car_y, landmarks_loc, noise=True) 79 | 80 | # create 1000 random particles and draw them on screen 81 | # particles = [] 82 | # for i in range(1000): 83 | # p_x = random.randint(10, world_size) 84 | # p_y = random.randint(10, world_size) 85 | # pygame.draw.circle(screen, red, (p_x, p_y), 2) 86 | # particles.append([p_x, p_y]) 87 | 88 | # move particles 89 | for i in range(1000): 90 | particles[i][0] += 10 91 | particles[i][1] -= 10 92 | particles[i][0] %= world_size 93 | particles[i][1] %= world_size 94 | pygame.draw.circle(screen, red, (particles[i][0] + car_length/2, particles[i][1] + car_width/2), 2) 95 | 96 | # assign weights to each particle according to measurement prob. 97 | weights = [] 98 | for i in range(1000): 99 | weights.append(measurement_prob(particles[i][0], particles[i][1], landmarks_loc, measurements)) 100 | 101 | # resampling 102 | p = [] 103 | # chose index from a uniform distribution 104 | index = int(random.random() * 1000) 105 | beta = 0.0 106 | mw = max(weights) 107 | for i in range(1000): 108 | # randomly chose between 0 and 2*w max 109 | beta += random.random()*2*mw 110 | while (weights[index] < beta): 111 | beta -= weights[index] 112 | index = (index + 1) % 1000 113 | # select current index 114 | p.append(particles[index]) 115 | 116 | particles = p 117 | 118 | for i in range(1000): 119 | 120 | pygame.draw.circle(screen, red, (particles[i][0] + car_length/2, particles[i][1] + car_width/2), particle_size) 121 | 122 | pygame.display.update() 123 | 124 | return car_x, car_y, landmarks_loc, particles 125 | 126 | 127 | 128 | def main_loop(): 129 | 130 | gameExit = False 131 | 132 | car_x = random.randint(10, world_size) 133 | car_y = random.randint(10, world_size) 134 | 135 | # draw car 136 | car(car_x, car_y) 137 | 138 | # create 1000 random particles and draw them on screen 139 | particles = [] 140 | for i in range(1000): 141 | p_x = random.randint(10, world_size) 142 | p_y = random.randint(10, world_size) 143 | pygame.draw.circle(screen, red, (p_x , p_y), 2) 144 | particles.append([p_x, p_y]) 145 | 146 | landmarks_loc = [] 147 | # create 4 random landmarks 148 | for _ in range(1): 149 | loc_x = random.randint(10, world_size) 150 | loc_y = random.randint(10, world_size) 151 | landmarks_loc.append([loc_x, loc_y]) 152 | 153 | # draw landmarks based on landmars_loc just found 154 | landmarks(landmarks_loc) 155 | 156 | pygame.display.update() 157 | 158 | car_x, car_y, landmarks_loc, particles = repeat(car_x, car_y, landmarks_loc, particles, 2) 159 | car_x, car_y, landmarks_loc, particles = repeat(car_x, car_y , landmarks_loc, particles, 8) 160 | car_x, car_y, landmarks_loc, particles = repeat(car_x, car_y, landmarks_loc, particles, 12) 161 | car_x, car_y, landmarks_loc, particles = repeat(car_x, car_y, landmarks_loc, particles, 12) 162 | 163 | 164 | while not gameExit: 165 | 166 | for event in pygame.event.get(): 167 | if event.type == pygame.QUIT: 168 | gameExit = True 169 | 170 | pygame.display.update() 171 | clock.tick(60) 172 | 173 | 174 | 175 | main_loop() 176 | 177 | -------------------------------------------------------------------------------- /planning/a-star.py: -------------------------------------------------------------------------------- 1 | # A visual demonstration of A* search algorithm 2 | 3 | from gridworld import GridWorld 4 | import time 5 | 6 | # build grid structure 7 | grid = [[0 for col in range(10)] for row in range(10)] 8 | grid[0][1] = 1 # obstacle 9 | grid[1][1] = 1 # obstacle 10 | grid[2][1] = 1 # obstacle 11 | grid[3][1] = 1 # obstacle 12 | grid[4][4] = 1 # obstacle 13 | 14 | init = [0, 0] 15 | goal = [4, 5] 16 | 17 | # build heuristics grid 18 | heuristics = [[0 for col in range(len(grid[0]))] for row in range(len(grid))] 19 | k = len(grid[0]) - 1 20 | for i in range(len(grid)-1, -1, -1): 21 | num = (len(grid[0])-1) - k 22 | for j in range(len(grid[0])-1, -1, -1): 23 | heuristics[i][j] = num 24 | num += 1 25 | k -= 1 26 | 27 | screen_size = 500 28 | cell_width = 45 29 | cell_height = 45 30 | cell_margin = 5 31 | 32 | gridworld = GridWorld(screen_size,cell_width, cell_height, cell_margin,init, goal, grid) 33 | 34 | def check_valid(node, grid): 35 | if node[0] >= 0 and node[0] < len(grid) and node[1] >= 0 and node[1] < len(grid[0]) and (grid[node[0]][node[1]] == 0): 36 | return True 37 | else: 38 | return False 39 | 40 | def heuristic(a, b): 41 | return (abs(a[0] - b[0]) + abs(a[1] - b[1])) 42 | 43 | def run_a_star(grid, heuristics, init, goal, cost): 44 | delta = [[-1, 0 ], # go up 45 | [ 0, -1], # go left 46 | [ 1, 0 ], # go down 47 | [ 0, 1]] # go right 48 | delta_name = ['^', '<', 'v', '>'] 49 | action = [[-1 for row in range(len(grid[0]))] for col in range(len(grid))] 50 | policy = [[' ' for row in range(len(grid[0]))] for col in range(len(grid))] 51 | expanded = [[-1 for row in range(len(grid[0]))] for col in range(len(grid))] 52 | 53 | visited = [] 54 | opened = [] 55 | 56 | # [f, g, [x, y]] 57 | # f = g + heuristics[x][y] 58 | # opened.append([0+heuristics[init[0]][init[1]], 0, init[0], init[1]]) 59 | opened.append([heuristic(goal, init), 0, init[0], init[1]]) 60 | 61 | visited.append([init[0], init[1]]) 62 | next = opened.pop() 63 | count = 0 64 | 65 | while [next[2],next[3]] != goal: 66 | 67 | if len(opened) > 0: 68 | opened.sort() 69 | print opened 70 | opened.reverse() 71 | next = opened.pop() 72 | 73 | x = next[2] 74 | y = next[3] 75 | g = next[1] 76 | f = next[0] 77 | gridworld.draw_cell([[f, [x, y]]]) 78 | gridworld.show() 79 | time.sleep(0.5) 80 | expanded[next[2]][next[3]] = count 81 | count += 1 82 | for a in range(len(delta)): 83 | x2 = x + delta[a][0] 84 | y2 = y + delta[a][1] 85 | if check_valid([x2, y2], grid): 86 | g2 = g + cost 87 | if [x2, y2] not in visited: 88 | #f = g2 + heuristics[x2][y2] 89 | f = g2 + heuristic(goal, [x2, y2]) 90 | opened.append([f,g2,x2, y2]) 91 | visited.append([x2, y2]) 92 | action[x2][y2] = a 93 | 94 | print expanded 95 | # policy search 96 | x = goal[0] 97 | y = goal[1] 98 | policy[x][y] = '*' 99 | path = [] 100 | path.append([x, y]) 101 | while([x, y] != init): 102 | x1 = x - delta[action[x][y]][0] 103 | y1 = y - delta[action[x][y]][1] 104 | policy[x1][y1] = delta_name[action[x][y]] 105 | x = x1 106 | y = y1 107 | path.append([x, y]) 108 | print policy 109 | path.reverse() 110 | print path 111 | smooth_path = gridworld.smooth_path(path) 112 | gridworld.draw_path(smooth_path) 113 | gridworld.show() 114 | 115 | run_a_star(grid, heuristics, init, goal, cost=1) 116 | 117 | 118 | 119 | 120 | 121 | -------------------------------------------------------------------------------- /planning/gridworld.py: -------------------------------------------------------------------------------- 1 | import pygame 2 | import time 3 | from copy import deepcopy 4 | 5 | class GridWorld: 6 | 7 | def __init__(self, screen_size,cell_width, 8 | cell_height, cell_margin,init, goal, grid): 9 | 10 | # define colors 11 | self.BLACK = (0, 0, 0) 12 | self.WHITE = (255, 255, 255) 13 | self.GREEN = (0, 255, 0) 14 | self.RED = (255, 0, 0) 15 | self.BLUE = (0, 0, 255) 16 | self.YELLOW = (255, 255, 0) 17 | 18 | # cell dimensions 19 | self.WIDTH = cell_width 20 | self.HEIGHT = cell_height 21 | self.MARGIN = cell_margin 22 | self.color = self.WHITE 23 | 24 | 25 | pygame.init() 26 | pygame.font.init() 27 | 28 | # set the width and height of the screen (width , height) 29 | self.size = (screen_size, screen_size) 30 | self.screen = pygame.display.set_mode(self.size) 31 | 32 | self.font = pygame.font.SysFont('arial', 20) 33 | 34 | pygame.display.set_caption("Grid world") 35 | 36 | self.clock = pygame.time.Clock() 37 | 38 | self.init = init 39 | self.goal = goal 40 | self.grid = grid 41 | 42 | 43 | self.screen.fill(self.BLACK) 44 | 45 | for row in range(len(grid)): 46 | for col in range(len(grid[0])): 47 | if [row, col] == self.init: 48 | self.color = self.GREEN 49 | elif [row, col] == self.goal: 50 | self.color = self.RED 51 | elif grid[row][col] == 1: 52 | self.color = self.BLACK 53 | else: 54 | self.color = self.WHITE 55 | pygame.draw.rect(self.screen, 56 | self.color, 57 | [(self.MARGIN + self.WIDTH)*col+self.MARGIN, 58 | (self.MARGIN + self.HEIGHT)*row+self.MARGIN, 59 | self.WIDTH, 60 | self.HEIGHT]) 61 | 62 | def text_objects(self, text, font): 63 | textSurface = font.render(text, True, self.BLACK) 64 | return textSurface, textSurface.get_rect() 65 | 66 | def draw_cell(self, nodes): 67 | for node in nodes: 68 | row = node[1][0] 69 | column = node[1][1] 70 | value = node[0] 71 | rect = pygame.draw.rect(self.screen, 72 | self.BLUE, 73 | [(self.MARGIN + self.WIDTH)*column+self.MARGIN, 74 | (self.MARGIN + self.HEIGHT)*row+self.MARGIN, 75 | self.WIDTH, 76 | self.HEIGHT]) 77 | TextSurf, TextRect = self.text_objects(str(value), self.font) 78 | TextRect.center = ((self.MARGIN + self.WIDTH)*column + 4*self.MARGIN, 79 | (self.MARGIN + self.HEIGHT)*row + 4*self.MARGIN) 80 | self.screen.blit(TextSurf, TextRect) 81 | 82 | def draw_shape(self, shape, center, size): 83 | origin = [0+1*self.MARGIN+22.5,0+1*self.MARGIN+22.5] 84 | col = self.MARGIN + self.WIDTH 85 | row = self.MARGIN + self.HEIGHT 86 | if shape == "circle": 87 | pygame.draw.circle(self.screen, self.RED, (int(origin[1]+row*(center[1])), int(origin[0]+col*(center[0]))), size) 88 | 89 | def draw_path(self, path): 90 | origin = [0+1*self.MARGIN+22.5,0+1*self.MARGIN+22.5] 91 | col = self.MARGIN + self.WIDTH 92 | row = self.MARGIN + self.HEIGHT 93 | pygame.draw.lines(self.screen, self.GREEN, False, [(origin[0]+col*i[1], origin[1]+row*i[0]) for i in path], 4) 94 | 95 | # smoothen the path 96 | def smooth_path(self,path,weight_data = 0.5, weight_smooth = 0.1, tolerance = 0.000001): 97 | #newpath = return_path 98 | newpath = deepcopy(path) 99 | change = tolerance 100 | while change >= tolerance: 101 | change = 0 102 | for i in range(1, len(path) - 1): 103 | for j in range(len(path[0])): 104 | d1 = weight_data*(path[i][j] - newpath[i][j]) 105 | d2 = weight_smooth*(newpath[i-1][j] + newpath[i+1][j] - 2*newpath[i][j]) 106 | change += abs(d1 + d2) 107 | newpath[i][j] += d1 + d2 108 | 109 | return newpath 110 | 111 | def show(self): 112 | pygame.display.flip() 113 | 114 | def loop(self): 115 | exit = False 116 | while exit == False: 117 | for event in pygame.event.get(): 118 | if event.type == pygame.QUIT: 119 | exit = True 120 | 121 | self.clock.tick(60) 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | -------------------------------------------------------------------------------- /planning/gridworld.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ioarun/pygame-robotics/5e966afaf69e843d3d96d21cedd0ee751b02cc76/planning/gridworld.pyc -------------------------------------------------------------------------------- /planning/planning-2.py: -------------------------------------------------------------------------------- 1 | # row, col pair in matrix is col, row in pygame 2 | 3 | import pygame 4 | import random 5 | import time 6 | 7 | 8 | display_width = 400 9 | display_height = 400 10 | 11 | red = (200, 0, 0) 12 | blue = (0, 0, 255) 13 | green = (0, 155, 0) 14 | yellow = (255, 255, 0) 15 | white = (255, 255, 255) 16 | black = (0, 0, 0) 17 | 18 | pygame.init() 19 | 20 | screen = pygame.display.set_mode((display_width, display_height)) 21 | 22 | clock = pygame.time.Clock() 23 | 24 | exit = False 25 | 26 | init = [0, 0] 27 | goal = [100, 100] 28 | landmarks_loc = [[30, 40], [30, 50], [0, 50], [0, 80], [0, 90], [0, 99]] 29 | 30 | def draw_path(path): 31 | pygame.draw.lines(screen, green, False, [(i[1], i[0]) for i in path], 2) 32 | 33 | def draw_landmarks(landmarks_loc): 34 | for i in (landmarks_loc): 35 | pygame.draw.circle(screen, blue, [i[1],i[0]], 4) 36 | 37 | def calculate_heuristics(a, b): 38 | return (abs(a[0] - b[0]) + abs(a[1] - b[1])) 39 | 40 | def check_valid_state(state, grid): 41 | x = state[0] 42 | y = state[1] 43 | if x >= 0 and x < len(grid) and y >= 0 and y < len(grid[0]) and grid[x][y] == 0: 44 | return True 45 | else: 46 | return False 47 | 48 | def run_a_star(init, goal, cost): 49 | delta = [[-1,0], # go up 50 | [0,-1], # go left 51 | [1,0], # go down 52 | [0,1]] # go right 53 | 54 | grid = [[0 for column in range(101)] for row in range(101)] 55 | # assign 1 for landmarks 56 | for l in landmarks_loc: 57 | grid[l[0]][l[1]] = 1 58 | 59 | 60 | action = [[-1 for row in range(len(grid[0]))] for col in range(len(grid))] 61 | opened = [] 62 | g = 0 63 | f = calculate_heuristics(init, goal) 64 | opened.append([f, g, init[0], init[1]]) 65 | next = opened.pop() 66 | visited = [] 67 | visited.append([next[2], next[3]]) 68 | 69 | while [next[2], next[3]] != goal: 70 | x = next[2] 71 | y = next[3] 72 | g = next[1] 73 | f = next[0] 74 | for a in range(len(delta)): 75 | x2 = x + delta[a][0] 76 | y2 = y + delta[a][1] 77 | if check_valid_state([x2, y2], grid): 78 | if [x2, y2] not in visited: 79 | g2 = g + cost 80 | f2 = g2 + calculate_heuristics([x2, y2], goal) 81 | opened.append([f2, g2, x2, y2]) 82 | visited.append([x2, y2]) 83 | action[x2][y2] = a 84 | 85 | if len(opened) > 0: 86 | opened.sort() 87 | opened.reverse() 88 | next = opened.pop() 89 | 90 | # policy search 91 | x = goal[0] 92 | y = goal[1] 93 | path = [] 94 | path.append([x, y]) 95 | while([x, y] != init): 96 | x1 = x - delta[action[x][y]][0] 97 | y1 = y - delta[action[x][y]][1] 98 | x = x1 99 | y = y1 100 | path.append([x, y]) 101 | path.reverse() 102 | return path 103 | 104 | 105 | 106 | while exit == False: 107 | 108 | for event in pygame.event.get(): 109 | if event.type == pygame.QUIT: 110 | exit = True 111 | 112 | screen.fill(white) 113 | pygame.draw.circle(screen, blue, init, 5) 114 | pygame.draw.circle(screen, red, goal, 5) 115 | draw_landmarks(landmarks_loc) 116 | path = run_a_star(init, goal, cost=1) 117 | draw_path(path) 118 | pygame.display.update() 119 | clock.tick(60) -------------------------------------------------------------------------------- /planning/planning.py: -------------------------------------------------------------------------------- 1 | # A visual demonstration of A* search algorithm 2 | 3 | from gridworld import GridWorld 4 | import time 5 | 6 | # build grid structure 7 | grid = [[0 for col in range(10)] for row in range(10)] 8 | grid[0][1] = 1 # obstacle 9 | grid[1][1] = 1 # obstacle 10 | grid[2][1] = 1 # obstacle 11 | grid[3][1] = 1 # obstacle 12 | grid[4][4] = 1 # obstacle 13 | 14 | init = [0, 0] 15 | goal = [4, 5] 16 | 17 | # build heuristics grid 18 | heuristics = [[0 for col in range(len(grid[0]))] for row in range(len(grid))] 19 | k = len(grid[0]) - 1 20 | for i in range(len(grid)-1, -1, -1): 21 | num = (len(grid[0])-1) - k 22 | for j in range(len(grid[0])-1, -1, -1): 23 | heuristics[i][j] = num 24 | num += 1 25 | k -= 1 26 | 27 | screen_size = 500 28 | cell_width = 45 29 | cell_height = 45 30 | cell_margin = 5 31 | 32 | gridworld = GridWorld(screen_size,cell_width, cell_height, cell_margin,init, goal, grid) 33 | 34 | def check_valid(node, grid): 35 | if node[0] >= 0 and node[0] < len(grid) and node[1] >= 0 and node[1] < len(grid[0]) and (grid[node[0]][node[1]] == 0): 36 | return True 37 | else: 38 | return False 39 | 40 | def heuristic(a, b): 41 | return (abs(a[0] - b[0]) + abs(a[1] - b[1])) 42 | 43 | def run_a_star(grid, heuristics, init, goal, cost): 44 | delta = [[-1, 0 ], # go up 45 | [ 0, -1], # go left 46 | [ 1, 0 ], # go down 47 | [ 0, 1]] # go right 48 | delta_name = ['^', '<', 'v', '>'] 49 | action = [[-1 for row in range(len(grid[0]))] for col in range(len(grid))] 50 | policy = [[' ' for row in range(len(grid[0]))] for col in range(len(grid))] 51 | expanded = [[-1 for row in range(len(grid[0]))] for col in range(len(grid))] 52 | 53 | visited = [] 54 | opened = [] 55 | 56 | # [f, g, [x, y]] 57 | # f = g + heuristics[x][y] 58 | # opened.append([0+heuristics[init[0]][init[1]], 0, init[0], init[1]]) 59 | opened.append([heuristic(goal, init), 0, init[0], init[1]]) 60 | 61 | visited.append([init[0], init[1]]) 62 | next = opened.pop() 63 | count = 0 64 | 65 | while [next[2],next[3]] != goal: 66 | 67 | if len(opened) > 0: 68 | opened.sort() 69 | # print opened 70 | opened.reverse() 71 | next = opened.pop() 72 | 73 | x = next[2] 74 | y = next[3] 75 | g = next[1] 76 | f = next[0] 77 | gridworld.draw_cell([[f, [x, y]]]) 78 | gridworld.show() 79 | expanded[next[2]][next[3]] = count 80 | count += 1 81 | for a in range(len(delta)): 82 | x2 = x + delta[a][0] 83 | y2 = y + delta[a][1] 84 | 85 | if check_valid([x2, y2], grid): 86 | g2 = g + cost 87 | if [x2, y2] not in visited: 88 | #f = g2 + heuristics[x2][y2] 89 | f = g2 + heuristic(goal, [x2, y2]) 90 | opened.append([f,g2,x2, y2]) 91 | visited.append([x2, y2]) 92 | action[x2][y2] = a 93 | 94 | # print expanded 95 | # policy search 96 | x = goal[0] 97 | y = goal[1] 98 | policy[x][y] = '*' 99 | path = [] 100 | path.append([x, y]) 101 | while([x, y] != init): 102 | x1 = x - delta[action[x][y]][0] 103 | y1 = y - delta[action[x][y]][1] 104 | policy[x1][y1] = delta_name[action[x][y]] 105 | x = x1 106 | y = y1 107 | path.append([x, y]) 108 | # print policy 109 | path.reverse() 110 | # print path 111 | smooth_path = gridworld.smooth_path(path) 112 | gridworld.draw_path(smooth_path) 113 | gridworld.show() 114 | return smooth_path 115 | 116 | smooth_path = run_a_star(grid, heuristics, init, goal, cost=1) 117 | for point in smooth_path: 118 | gridworld.draw_shape("circle", point, 8) 119 | gridworld.show() 120 | time.sleep(1) 121 | gridworld.loop() 122 | 123 | 124 | 125 | 126 | 127 | -------------------------------------------------------------------------------- /slither/apple.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ioarun/pygame-robotics/5e966afaf69e843d3d96d21cedd0ee751b02cc76/slither/apple.png -------------------------------------------------------------------------------- /slither/eat.wav: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ioarun/pygame-robotics/5e966afaf69e843d3d96d21cedd0ee751b02cc76/slither/eat.wav -------------------------------------------------------------------------------- /slither/eat2.wav: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ioarun/pygame-robotics/5e966afaf69e843d3d96d21cedd0ee751b02cc76/slither/eat2.wav -------------------------------------------------------------------------------- /slither/setup.py: -------------------------------------------------------------------------------- 1 | # buid : python setup.py build 2 | # installer : python setup.py bdist_msi 3 | 4 | import cx_Freeze 5 | 6 | executables = [cx_Freeze.Executable("slither.py")] 7 | 8 | cx_Freeze.setup( 9 | name="Slither", 10 | options={"build_exe":{"packages":["pygame"], "include_files":["apple.png", "snake.png"]}}, 11 | description = "Slither Game", 12 | executables = executables 13 | ) -------------------------------------------------------------------------------- /slither/slither.py: -------------------------------------------------------------------------------- 1 | # Snake eats apple and gets bigger 2 | 3 | import pygame 4 | import time 5 | import random 6 | 7 | pygame.init() 8 | 9 | white = (255, 255, 255) 10 | black = (0, 0, 0) 11 | red = (255, 0, 0) 12 | green = (0, 155, 0) 13 | 14 | display_width = 800 15 | display_height = 600 16 | 17 | gameDisplay = pygame.display.set_mode((800, 600)) 18 | pygame.display.set_caption("Slither") 19 | 20 | img = pygame.image.load('snake.png') 21 | apple = pygame.image.load('apple.png') 22 | 23 | eat_sound = pygame.mixer.Sound("eat2.wav") 24 | 25 | icon = pygame.image.load('apple.png') 26 | pygame.display.set_icon(icon) 27 | 28 | AppleThickness = 30 29 | block_size = 20 30 | clock = pygame.time.Clock() 31 | 32 | FPS = 10 33 | 34 | direction = "right" 35 | 36 | smallfont = pygame.font.SysFont("arial", 25) 37 | medfont = pygame.font.SysFont("arial", 50) 38 | largefont = pygame.font.SysFont("arial", 80) 39 | 40 | def pause(): 41 | paused = True 42 | message_to_screen("Paused", 43 | black, 44 | -100, 45 | size="large") 46 | message_to_screen("Press C to continue or Q to continue.", 47 | black, 48 | 25) 49 | pygame.display.update() 50 | while paused: 51 | for event in pygame.event.get(): 52 | if event.type == pygame.QUIT: 53 | pygame.quit() 54 | quit() 55 | if event.type == pygame.KEYDOWN: 56 | if event.key == pygame.K_c: 57 | paused = False 58 | 59 | elif event.key == pygame.K_q: 60 | pygame.quit() 61 | quit() 62 | 63 | # gameDisplay.fill(white) 64 | 65 | clock.tick(5) 66 | 67 | def score(score): 68 | text = smallfont.render("Score: "+str(score), True, black) 69 | gameDisplay.blit(text, [0, 0]) 70 | 71 | def randAppleGen(): 72 | randAppleX = round(random.randrange(0, display_width-AppleThickness))#/10.0)*10.0 73 | randAppleY = round(random.randrange(0, display_height-AppleThickness))#/10.0)*10.0 74 | return randAppleX, randAppleY 75 | 76 | def game_intro(): 77 | 78 | intro = True 79 | 80 | while intro: 81 | 82 | for event in pygame.event.get(): 83 | if event.type == pygame.QUIT: 84 | pygame.quit() 85 | quit() 86 | 87 | if event.type == pygame.KEYDOWN: 88 | if event.key == pygame.K_c: 89 | intro = False 90 | if event.key == pygame.K_q: 91 | pygame.quit() 92 | quit() 93 | 94 | gameDisplay.fill(white) 95 | message_to_screen("Welcome to Slither", 96 | green, 97 | -100, 98 | "large") 99 | 100 | message_to_screen("The objective of the game is to eat red apples.", 101 | black, 102 | -30) 103 | message_to_screen("The more apples you eat, the longer you get.", 104 | black, 105 | 10) 106 | message_to_screen("If you run into yourself, or the edges, you die.", 107 | black, 108 | 50) 109 | 110 | message_to_screen("Press C to play, P to pause or Q to quit.", 111 | black, 112 | 180) 113 | 114 | pygame.display.update() 115 | clock.tick(15) 116 | 117 | 118 | def snake(block_size, snakelist): 119 | if direction == "right": 120 | head = pygame.transform.rotate(img, 270) 121 | elif direction == "left": 122 | head = pygame.transform.rotate(img, 90) 123 | elif direction == "up": 124 | head = img 125 | elif direction == "down": 126 | head = pygame.transform.rotate(img, 180) 127 | 128 | gameDisplay.blit(head, (snakelist[-1][0], snakelist[-1][1])) 129 | for x_y in snakelist[:-1]: 130 | pygame.draw.rect(gameDisplay, green, [x_y[0], x_y[1], block_size, block_size]) 131 | 132 | def text_objects(text, color, size): 133 | if size == "small": 134 | textSurface = smallfont.render(text, True, color) 135 | elif size == "medium": 136 | textSurface = medfont.render(text, True, color) 137 | elif size == "large": 138 | textSurface = largefont.render(text, True, color) 139 | 140 | return textSurface, textSurface.get_rect() 141 | 142 | 143 | def message_to_screen(msg, color, y_displace=0, size="small"): 144 | # screen_text = font.render(msg,True, color) 145 | # gameDisplay.blit(screen_text, [display_width/2, display_height/2]) 146 | textSurface, textRect = text_objects(msg, color, size) 147 | textRect.center = display_width/2 , display_height/2 + y_displace 148 | gameDisplay.blit(textSurface, textRect) 149 | 150 | # game loop 151 | def gameLoop(): 152 | global direction 153 | direction = "right" 154 | 155 | gameExit = False 156 | gameOver = False 157 | 158 | lead_x = display_width/2 159 | lead_y = display_height/2 160 | 161 | lead_x_change = 10 162 | lead_y_change = 0 163 | 164 | snakelist = [] 165 | 166 | snakeLength = 1 167 | 168 | randAppleX, randAppleY = randAppleGen() 169 | 170 | while not gameExit: 171 | 172 | if gameOver == True: 173 | message_to_screen("Game over.", red, y_displace=-50, size="large") 174 | message_to_screen("Press C to play again or Q to quit", 175 | black, y_displace=50, size="medium") 176 | pygame.display.update() 177 | 178 | while gameOver == True: 179 | # gameDisplay.fill(white) 180 | 181 | for event in pygame.event.get(): 182 | if event.type == pygame.QUIT: 183 | gameExit = True 184 | gameOver = False 185 | if event.type == pygame.KEYDOWN: 186 | if event.key == pygame.K_q: 187 | gameExit = True 188 | gameOver = False 189 | if event.key == pygame.K_c: 190 | gameLoop() 191 | 192 | for event in pygame.event.get(): 193 | if event.type == pygame.QUIT: 194 | gameExit = True 195 | if event.type == pygame.KEYDOWN: 196 | if event.key == pygame.K_LEFT: 197 | direction = "left" 198 | lead_x_change = -block_size 199 | lead_y_change = 0 200 | elif event.key == pygame.K_RIGHT: 201 | direction = "right" 202 | lead_x_change = block_size 203 | lead_y_change = 0 204 | elif event.key == pygame.K_UP: 205 | direction = "up" 206 | lead_y_change = -block_size 207 | lead_x_change = 0 208 | elif event.key == pygame.K_DOWN: 209 | direction = "down" 210 | lead_y_change = block_size 211 | lead_x_change = 0 212 | elif event.key == pygame.K_p: 213 | pause() 214 | 215 | if lead_x >= display_width or lead_x < 0 or lead_y >= display_height or lead_y < 0: 216 | gameOver = True 217 | 218 | lead_x += lead_x_change 219 | lead_y += lead_y_change 220 | 221 | gameDisplay.fill(white) 222 | 223 | # pygame.draw.rect(gameDisplay, red, [randAppleX, randAppleY, AppleThickness, AppleThickness]) 224 | gameDisplay.blit(apple, (randAppleX, randAppleY)) 225 | 226 | snakeHead = [] 227 | snakeHead.append(lead_x) 228 | snakeHead.append(lead_y) 229 | snakelist.append(snakeHead) 230 | 231 | if len(snakelist) > snakeLength: 232 | del snakelist[0] 233 | 234 | for eachSegment in snakelist[:-1]: 235 | if eachSegment == snakeHead: 236 | gameOver = True 237 | 238 | snake(block_size, snakelist) 239 | 240 | score(snakeLength-1) 241 | 242 | pygame.display.update() 243 | 244 | # # cross-over/collision-detection code 245 | # if lead_x >= randAppleX and lead_x <= randAppleX + AppleThickness: 246 | # if lead_y >= randAppleY and lead_y <= randAppleY + AppleThickness: 247 | # randAppleX = round(random.randrange(0, display_width-block_size))#/10.0)*10.0 248 | # randAppleY = round(random.randrange(0, display_height-block_size))#/10.0)*10.0 249 | # snakeLength += 1 250 | 251 | if lead_x > randAppleX and lead_x < randAppleX + AppleThickness or lead_x + block_size > randAppleX and lead_x + block_size < randAppleX + AppleThickness: 252 | if lead_y > randAppleY and lead_y < randAppleY + AppleThickness or lead_y + block_size > randAppleY and lead_y + block_size < randAppleY + AppleThickness: 253 | randAppleX, randAppleY = randAppleGen() 254 | pygame.mixer.Sound.play(eat_sound) 255 | snakeLength += 1 256 | 257 | clock.tick(FPS) 258 | 259 | pygame.quit() 260 | quit() 261 | 262 | game_intro() 263 | gameLoop() -------------------------------------------------------------------------------- /slither/snake.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ioarun/pygame-robotics/5e966afaf69e843d3d96d21cedd0ee751b02cc76/slither/snake.png -------------------------------------------------------------------------------- /tank-game/explosion.wav: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ioarun/pygame-robotics/5e966afaf69e843d3d96d21cedd0ee751b02cc76/tank-game/explosion.wav -------------------------------------------------------------------------------- /tank-game/gunshot1.wav: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ioarun/pygame-robotics/5e966afaf69e843d3d96d21cedd0ee751b02cc76/tank-game/gunshot1.wav -------------------------------------------------------------------------------- /tank-game/gunshot2.wav: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ioarun/pygame-robotics/5e966afaf69e843d3d96d21cedd0ee751b02cc76/tank-game/gunshot2.wav -------------------------------------------------------------------------------- /tank-game/shape_of_you_ed_sheeran.mp3: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ioarun/pygame-robotics/5e966afaf69e843d3d96d21cedd0ee751b02cc76/tank-game/shape_of_you_ed_sheeran.mp3 -------------------------------------------------------------------------------- /tank-game/tank-game.py: -------------------------------------------------------------------------------- 1 | # The tank game - shoot and destroy the enemy tank 2 | 3 | import pygame 4 | import time 5 | import random 6 | 7 | pygame.init() 8 | 9 | white = (255, 255, 255) 10 | black = (0, 0, 0) 11 | red = (200, 0, 0) 12 | light_red = (255, 0, 0) 13 | 14 | yellow = (200, 200, 0) 15 | light_yellow = (255, 255, 0) 16 | 17 | green = (0, 155, 0) 18 | light_green = (0, 255, 0) 19 | 20 | display_width = 800 21 | display_height = 600 22 | 23 | tankHeight = 20 24 | tankWidth = 40 25 | 26 | ground_height = 35 27 | 28 | 29 | gameDisplay = pygame.display.set_mode((800, 600)) 30 | pygame.display.set_caption("Tanks") 31 | 32 | fire_sound = pygame.mixer.Sound("gunshot2.wav") 33 | explosion_sound = pygame.mixer.Sound("explosion.wav") 34 | 35 | pygame.mixer.music.load("shape_of_you_ed_sheeran.mp3") 36 | # loop forever 37 | pygame.mixer.music.play(-1) 38 | 39 | clock = pygame.time.Clock() 40 | 41 | FPS = 10 42 | 43 | direction = "right" 44 | 45 | smallfont = pygame.font.SysFont("arial", 25) 46 | medfont = pygame.font.SysFont("arial", 50) 47 | largefont = pygame.font.SysFont("arial", 80) 48 | 49 | def tank(x, y, turPos): 50 | x = int(x) 51 | y = int(y) 52 | 53 | possibleTurrets = [(x-27, y - 2), 54 | (x-26, y - 5), 55 | (x-25, y - 8), 56 | (x-23, y - 12), 57 | (x-20, y - 14), 58 | (x-18, y - 15), 59 | (x-15, y - 17), 60 | (x-13, y - 19), 61 | (x-11, y-21)] 62 | 63 | pygame.draw.circle(gameDisplay, black, (x, y), int(tankHeight/2)) 64 | pygame.draw.rect(gameDisplay, black, (x - tankWidth/2, y, tankWidth, tankHeight)) 65 | 66 | pygame.draw.line(gameDisplay, black, (x, y), possibleTurrets[turPos], 5) 67 | 68 | startX = 15 69 | # add wheels 70 | for i in range(7): 71 | pygame.draw.circle(gameDisplay, black, (x - startX, y + tankHeight), 5) 72 | startX -= 5 73 | 74 | # return turret position 75 | return possibleTurrets[turPos] 76 | 77 | def enemy_tank(x, y, turPos): 78 | x = int(x) 79 | y = int(y) 80 | 81 | possibleTurrets = [(x+27, y - 2), 82 | (x+26, y - 5), 83 | (x+25, y - 8), 84 | (x+23, y - 12), 85 | (x+20, y - 14), 86 | (x+18, y - 15), 87 | (x+15, y - 17), 88 | (x+13, y - 19), 89 | (x+11, y-21)] 90 | 91 | pygame.draw.circle(gameDisplay, black, (x, y), int(tankHeight/2)) 92 | pygame.draw.rect(gameDisplay, black, (x - tankWidth/2, y, tankWidth, tankHeight)) 93 | 94 | pygame.draw.line(gameDisplay, black, (x, y), possibleTurrets[turPos], 5) 95 | 96 | startX = 15 97 | # add wheels 98 | for i in range(7): 99 | pygame.draw.circle(gameDisplay, black, (x - startX, y + tankHeight), 5) 100 | startX -= 5 101 | 102 | # return turret position 103 | return possibleTurrets[turPos] 104 | 105 | def pause(): 106 | paused = True 107 | message_to_screen("Paused", 108 | black, 109 | -100, 110 | size="large") 111 | message_to_screen("Press C to continue or Q to continue.", 112 | black, 113 | 25) 114 | pygame.display.update() 115 | while paused: 116 | for event in pygame.event.get(): 117 | if event.type == pygame.QUIT: 118 | pygame.quit() 119 | quit() 120 | if event.type == pygame.KEYDOWN: 121 | if event.key == pygame.K_c: 122 | paused = False 123 | 124 | elif event.key == pygame.K_q: 125 | pygame.quit() 126 | quit() 127 | 128 | # gameDisplay.fill(white) 129 | 130 | clock.tick(5) 131 | 132 | def score(score): 133 | text = smallfont.render("Score: "+str(score), True, black) 134 | gameDisplay.blit(text, [0, 0]) 135 | 136 | 137 | def game_controls(): 138 | 139 | gcont = True 140 | 141 | while gcont: 142 | 143 | for event in pygame.event.get(): 144 | if event.type == pygame.QUIT: 145 | pygame.quit() 146 | quit() 147 | 148 | 149 | gameDisplay.fill(white) 150 | message_to_screen("Controls", 151 | green, 152 | -100, 153 | "large") 154 | 155 | message_to_screen("Fire: Spacebar", 156 | black, 157 | -30) 158 | message_to_screen("Move Turret: Up and Down arrows", 159 | black, 160 | 10) 161 | message_to_screen("Move Tank: Left and Right arrows", 162 | black, 163 | 50) 164 | 165 | message_to_screen("Pause: P", 166 | black, 167 | 90) 168 | 169 | button("play", 150, 500, 100, 50, green, light_green, action="play") 170 | button("Main", 350, 500, 100, 50, yellow, light_yellow, action="main") 171 | button("quit", 550, 500, 100, 50, red, light_red, action="quit") 172 | 173 | pygame.display.update() 174 | clock.tick(15) 175 | 176 | 177 | 178 | def barrier(xlocation, randomHeight, barrier_width): 179 | pygame.draw.rect(gameDisplay, black,[xlocation, display_height-randomHeight, barrier_width, randomHeight]) 180 | 181 | def explosion(x, y, size=50): 182 | pygame.mixer.Sound.play(explosion_sound) 183 | explode = True 184 | 185 | while explode: 186 | for event in pygame.event.get(): 187 | if event.type == pygame.QUIT: 188 | pygame.quit() 189 | quit() 190 | 191 | startPoint = x, y 192 | 193 | colorChoices = [red, light_red, yellow, light_yellow] 194 | 195 | magnitude = 1 196 | 197 | while magnitude < size: 198 | exploding_bit_x = x + random.randrange(-1*magnitude, magnitude) 199 | exploding_bit_y = y + random.randrange(-1*magnitude, magnitude) 200 | 201 | pygame.draw.circle(gameDisplay, colorChoices[random.randrange(0, 4)],(exploding_bit_x, exploding_bit_y), random.randrange(1, 5)) 202 | magnitude += 1 203 | 204 | pygame.display.update() 205 | clock.tick(100) 206 | 207 | explode =False 208 | 209 | def fireShell(xy, tankx, tanky, turPos, gun_power): 210 | fire = True 211 | 212 | startingShell = list(xy) # from tuple to list conversion 213 | # print ("FIRE!", xy) 214 | 215 | while fire: 216 | for event in pygame.event.get(): 217 | if event.type == pygame.QUIT: 218 | pygame.quit() 219 | quit() 220 | 221 | # print startingShell[0], startingShell[1] 222 | pygame.draw.circle(gameDisplay, red, (startingShell[0], startingShell[1]), 5) 223 | 224 | startingShell[0] -= (12 - turPos)*2 225 | 226 | # y = x**2 227 | startingShell[1] += int((((startingShell[0] - xy[0])*0.015)**2) - (turPos + turPos/(12-turPos))) 228 | 229 | if startingShell[1] > display_height: 230 | fire = False 231 | 232 | pygame.display.update() 233 | clock.tick(60) 234 | 235 | def fireShell2(xy, tankx, tanky, turPos, gun_power, xlocation, barrier_width, randomHeight, enemyTankX, enemyTankY): 236 | 237 | pygame.mixer.Sound.play(fire_sound) 238 | 239 | fire = True 240 | damage = 0 241 | startingShell = list(xy) # from tuple to list conversion 242 | # print ("FIRE!", xy) 243 | while fire: 244 | for event in pygame.event.get(): 245 | if event.type == pygame.QUIT: 246 | pygame.quit() 247 | quit() 248 | 249 | # print startingShell[0], startingShell[1] 250 | pygame.draw.circle(gameDisplay, red, (startingShell[0], startingShell[1]), 5) 251 | 252 | startingShell[0] -= (12 - turPos)*2 253 | 254 | # y = x**2 255 | startingShell[1] += int((((startingShell[0] - xy[0])*0.015/(gun_power/50.0))**2) - (turPos + turPos/(12-turPos))) 256 | 257 | if startingShell[1] > display_height - ground_height: 258 | hit_x = int((startingShell[0]*display_height-ground_height)/startingShell[1]) 259 | hit_y = int(display_height-ground_height) 260 | print "Last shell:", startingShell[0], startingShell[1] 261 | print "Impact:", hit_x, hit_y 262 | 263 | if enemyTankX + 10 > hit_x > enemyTankX - 10: 264 | print "critical hit" 265 | damage = 25 266 | 267 | elif enemyTankX + 15 > hit_x > enemyTankX - 15: 268 | print "hard hit" 269 | damage = 18 270 | 271 | elif enemyTankX + 25 > hit_x > enemyTankX - 25: 272 | print "medium hit" 273 | damage = 10 274 | 275 | elif enemyTankX + 35 > hit_x > enemyTankX - 35: 276 | print "light hit" 277 | damage = 5 278 | 279 | explosion(hit_x, hit_y) 280 | fire = False 281 | 282 | 283 | check_x_1 = startingShell[0] <= xlocation + barrier_width 284 | check_x_2 = startingShell[0] >= xlocation 285 | 286 | check_y_1 = startingShell[1] <= display_height 287 | check_y_2 = startingShell[1] >= display_height -randomHeight 288 | 289 | if check_x_1 and check_x_2 and check_y_1 and check_y_2: 290 | hit_x = int(startingShell[0]) 291 | hit_y = int(startingShell[1]) 292 | print "Last shell:", startingShell[0], startingShell[1] 293 | print "Impact:", hit_x, hit_y 294 | explosion(hit_x, hit_y) 295 | fire = False 296 | 297 | pygame.display.update() 298 | clock.tick(60) 299 | 300 | return damage 301 | 302 | 303 | def e_fireShell(xy, tankx, tanky, turPos, gun_power, xlocation, barrier_width, randomHeight, ptankX, ptankY): 304 | 305 | pygame.mixer.Sound.play(fire_sound) 306 | 307 | damage = 0 308 | 309 | currentPower = 1 310 | power_found = False 311 | 312 | while not power_found: 313 | currentPower += 1 314 | if currentPower > 100: 315 | power_found = True 316 | fire = True 317 | 318 | startingShell = list(xy) # from tuple to list conversion (just one element) 319 | # print ("FIRE!", xy) 320 | while fire: 321 | for event in pygame.event.get(): 322 | if event.type == pygame.QUIT: 323 | pygame.quit() 324 | quit() 325 | 326 | # print startingShell[0], startingShell[1] 327 | # pygame.draw.circle(gameDisplay, red, (startingShell[0], startingShell[1]), 5) 328 | 329 | startingShell[0] += (12 - turPos)*2 330 | 331 | # y = x**2 332 | startingShell[1] += int((((startingShell[0] - xy[0])*0.015/(currentPower/50.0))**2) - (turPos + turPos/(12-turPos))) 333 | 334 | if startingShell[1] > display_height - ground_height: 335 | hit_x = int((startingShell[0]*display_height-ground_height)/startingShell[1]) 336 | hit_y = int(display_height-ground_height) 337 | # explosion(hit_x, hit_y) 338 | # if found tank 339 | if ptankX + 15 > hit_x > ptankX - 15: 340 | print "Target acquired!" 341 | power_found = True 342 | fire = False 343 | 344 | 345 | check_x_1 = startingShell[0] <= xlocation + barrier_width 346 | check_x_2 = startingShell[0] >= xlocation 347 | 348 | check_y_1 = startingShell[1] <= display_height 349 | check_y_2 = startingShell[1] >= display_height -randomHeight 350 | 351 | if check_x_1 and check_x_2 and check_y_1 and check_y_2: 352 | hit_x = int(startingShell[0]) 353 | hit_y = int(startingShell[1]) 354 | # explosion(hit_x, hit_y) 355 | fire = False 356 | 357 | 358 | fire = True 359 | 360 | startingShell = list(xy) # from tuple to list conversion 361 | # print ("FIRE!", xy) 362 | while fire: 363 | for event in pygame.event.get(): 364 | if event.type == pygame.QUIT: 365 | pygame.quit() 366 | quit() 367 | 368 | # print startingShell[0], startingShell[1] 369 | pygame.draw.circle(gameDisplay, red, (startingShell[0], startingShell[1]), 5) 370 | 371 | startingShell[0] += (12 - turPos)*2 372 | 373 | gun_power = random.randrange(int(currentPower*0.90), int(currentPower*1.10)) 374 | 375 | # y = x**2 376 | startingShell[1] += int((((startingShell[0] - xy[0])*0.015/(gun_power/50.0))**2) - (turPos + turPos/(12-turPos))) 377 | 378 | if startingShell[1] > display_height - ground_height: 379 | hit_x = int((startingShell[0]*display_height-ground_height)/startingShell[1]) 380 | hit_y = int(display_height-ground_height) 381 | print "Last shell:", startingShell[0], startingShell[1] 382 | print "Impact:", hit_x, hit_y 383 | 384 | if ptankX + 10 > hit_x > ptankX - 10: 385 | print "critical hit" 386 | damage = 25 387 | 388 | elif ptankX + 15 > hit_x > ptankX - 15: 389 | print "hard hit" 390 | damage = 18 391 | 392 | elif ptankX + 25 > hit_x > ptankX - 25: 393 | print "medium hit" 394 | damage = 10 395 | 396 | elif ptankX + 35 > hit_x > ptankX - 35: 397 | print "light hit" 398 | damage = 5 399 | explosion(hit_x, hit_y) 400 | fire = False 401 | 402 | 403 | check_x_1 = startingShell[0] <= xlocation + barrier_width 404 | check_x_2 = startingShell[0] >= xlocation 405 | 406 | check_y_1 = startingShell[1] <= display_height 407 | check_y_2 = startingShell[1] >= display_height -randomHeight 408 | 409 | if check_x_1 and check_x_2 and check_y_1 and check_y_2: 410 | hit_x = int(startingShell[0]) 411 | hit_y = int(startingShell[1]) 412 | print "Last shell:", startingShell[0], startingShell[1] 413 | print "Impact:", hit_x, hit_y 414 | explosion(hit_x, hit_y) 415 | fire = False 416 | 417 | 418 | 419 | pygame.display.update() 420 | clock.tick(60) 421 | 422 | return damage 423 | 424 | 425 | 426 | def power(level): 427 | text = smallfont.render("Power: "+str(level)+"%",True, black) 428 | gameDisplay.blit(text, [display_width/2, 0]) 429 | 430 | def game_intro(): 431 | 432 | intro = True 433 | 434 | while intro: 435 | 436 | for event in pygame.event.get(): 437 | if event.type == pygame.QUIT: 438 | pygame.quit() 439 | quit() 440 | 441 | if event.type == pygame.KEYDOWN: 442 | if event.key == pygame.K_c: 443 | intro = False 444 | if event.key == pygame.K_q: 445 | pygame.quit() 446 | quit() 447 | 448 | gameDisplay.fill(white) 449 | message_to_screen("Welcome to Tanks!", 450 | green, 451 | -100, 452 | "large") 453 | 454 | message_to_screen("The objective of the game is to shoot and destroy.", 455 | black, 456 | -30) 457 | message_to_screen("the enemy tank before they destroy you.", 458 | black, 459 | 10) 460 | message_to_screen("The more enemies you destroy, the harder they get.", 461 | black, 462 | 50) 463 | 464 | button("play", 150, 500, 100, 50, green, light_green, action="play") 465 | button("controls", 350, 500, 100, 50, yellow, light_yellow, action="controls") 466 | button("quit", 550, 500, 100, 50, red, light_red, action="quit") 467 | 468 | pygame.display.update() 469 | clock.tick(15) 470 | 471 | def game_over(): 472 | 473 | game_over = True 474 | 475 | while game_over: 476 | 477 | for event in pygame.event.get(): 478 | if event.type == pygame.QUIT: 479 | pygame.quit() 480 | quit() 481 | 482 | gameDisplay.fill(white) 483 | message_to_screen("Game Over", 484 | green, 485 | -100, 486 | "large") 487 | 488 | message_to_screen("You died.", 489 | black, 490 | -30) 491 | 492 | button("play again", 150, 500, 150, 50, green, light_green, action="play") 493 | button("controls", 350, 500, 100, 50, yellow, light_yellow, action="controls") 494 | button("quit", 550, 500, 100, 50, red, light_red, action="quit") 495 | 496 | pygame.display.update() 497 | clock.tick(15) 498 | 499 | def you_win(): 500 | 501 | win = True 502 | 503 | while win: 504 | 505 | for event in pygame.event.get(): 506 | if event.type == pygame.QUIT: 507 | pygame.quit() 508 | quit() 509 | 510 | gameDisplay.fill(white) 511 | message_to_screen("You won!", 512 | green, 513 | -100, 514 | "large") 515 | 516 | message_to_screen("Congratulations!", 517 | black, 518 | -30) 519 | 520 | button("play again", 150, 500, 150, 50, green, light_green, action="play") 521 | button("controls", 350, 500, 100, 50, yellow, light_yellow, action="controls") 522 | button("quit", 550, 500, 100, 50, red, light_red, action="quit") 523 | 524 | pygame.display.update() 525 | clock.tick(15) 526 | 527 | def text_objects(text, color, size): 528 | if size == "small": 529 | textSurface = smallfont.render(text, True, color) 530 | elif size == "medium": 531 | textSurface = medfont.render(text, True, color) 532 | elif size == "large": 533 | textSurface = largefont.render(text, True, color) 534 | 535 | return textSurface, textSurface.get_rect() 536 | 537 | def text_to_button(msg, color, buttonx, buttony, buttonwidth, buttonheight,size = "small"): 538 | textSurface, textRect = text_objects(msg, color, size) 539 | textRect.center = ((buttonx+(buttonwidth/2)), buttony+(buttonheight/2)) 540 | gameDisplay.blit(textSurface, textRect) 541 | 542 | def button(text, x, y, width, height, inactive_color, active_color, action=None): 543 | cur = pygame.mouse.get_pos() 544 | # returns a tuple (is_left?, is_center?, is_right?) 545 | click = pygame.mouse.get_pressed() 546 | 547 | if x + width > cur[0] > x and y + height > cur[1] > y: 548 | pygame.draw.rect(gameDisplay, active_color, (x, y, width, height)) 549 | 550 | if click[0] == 1 and action != None: 551 | if action == "quit": 552 | pygame.quit() 553 | quit() 554 | if action == "controls": 555 | game_controls() 556 | if action == "play": 557 | gameLoop() 558 | if action == "main": 559 | game_intro() 560 | 561 | else: 562 | pygame.draw.rect(gameDisplay, inactive_color, (x, y, width, height)) 563 | 564 | text_to_button(text, black, x, y, width, height) 565 | 566 | def message_to_screen(msg, color, y_displace=0, size="small"): 567 | # screen_text = font.render(msg,True, color) 568 | # gameDisplay.blit(screen_text, [display_width/2, display_height/2]) 569 | textSurface, textRect = text_objects(msg, color, size) 570 | textRect.center = display_width/2 , display_height/2 + y_displace 571 | gameDisplay.blit(textSurface, textRect) 572 | 573 | 574 | def health_bars(player_health, enemy_health): 575 | if player_health > 75: 576 | player_health_color = green 577 | elif player_health > 50: 578 | player_health_color = yellow 579 | else: 580 | player_health_color = red 581 | 582 | if enemy_health > 75: 583 | enemy_health_color = green 584 | elif enemy_health > 50: 585 | enemy_health_color = yellow 586 | else: 587 | enemy_health_color = red 588 | 589 | pygame.draw.rect(gameDisplay, player_health_color, (680, 25, player_health, 25)) 590 | pygame.draw.rect(gameDisplay, enemy_health_color, (20, 25, enemy_health, 25)) 591 | 592 | 593 | # game loop 594 | def gameLoop(): 595 | global direction 596 | direction = "right" 597 | 598 | gameExit = False 599 | gameOver = False 600 | 601 | player_health = 100 602 | enemy_health = 100 603 | 604 | # main tank position on screen 605 | mainTankX = display_width*0.9 606 | mainTankY = display_height*0.9 607 | 608 | # enemy tank position on screen 609 | enemyTankX = display_width*0.1 610 | enemyTankY = display_height*0.9 611 | 612 | tankMove = 0 613 | 614 | currentTurPos = 0 615 | changeTur = 0 616 | 617 | fire_power = 50 618 | power_change = 0 619 | 620 | xlocation = (display_width/2)+random.randint(-0.1*display_width,0.1*display_width) 621 | randomHeight = random.randrange(display_height*0.1, display_height*0.6) 622 | 623 | barrier_width = 50 624 | 625 | while not gameExit: 626 | 627 | if gameOver == True: 628 | message_to_screen("Game over.", red, y_displace=-50, size="large") 629 | message_to_screen("Press C to play again or Q to quit", 630 | black, y_displace=50, size="medium") 631 | pygame.display.update() 632 | 633 | while gameOver == True: 634 | # gameDisplay.fill(white) 635 | 636 | for event in pygame.event.get(): 637 | if event.type == pygame.QUIT: 638 | gameExit = True 639 | gameOver = False 640 | if event.type == pygame.KEYDOWN: 641 | if event.key == pygame.K_q: 642 | gameExit = True 643 | gameOver = False 644 | if event.key == pygame.K_c: 645 | gameLoop() 646 | 647 | for event in pygame.event.get(): 648 | if event.type == pygame.QUIT: 649 | gameExit = True 650 | if event.type == pygame.KEYDOWN: 651 | if event.key == pygame.K_LEFT: 652 | tankMove = -5 653 | elif event.key == pygame.K_RIGHT: 654 | tankMove = +5 655 | elif event.key == pygame.K_UP: 656 | changeTur = 1 657 | elif event.key == pygame.K_DOWN: 658 | changeTur = -1 659 | elif event.key == pygame.K_p: 660 | pause() 661 | elif event.key == pygame.K_SPACE: 662 | # argument = turret location 663 | # fireShell(gun, mainTankX, mainTankY, currentTurPos, fire_power) 664 | damage = fireShell2(gun, mainTankX, mainTankY, currentTurPos, fire_power, xlocation, barrier_width, randomHeight, enemyTankX, enemyTankY) 665 | enemy_health -= damage 666 | 667 | 668 | possibleMovement = ['f', 'r'] 669 | moveIndex = random.randrange(0, 2) 670 | for x in range(random.randrange(0,10)): 671 | if display_width*0.3 > enemyTankX > display_width*0.03: 672 | if possibleMovement[moveIndex] == 'f': 673 | enemyTankX += 5 674 | elif possibleMovement[moveIndex] == 'r': 675 | enemyTankX -= 5 676 | 677 | gameDisplay.fill(white) 678 | health_bars(player_health, enemy_health) 679 | gun = tank(mainTankX, mainTankY, currentTurPos) 680 | enemy_gun = enemy_tank(enemyTankX, enemyTankY, 8) 681 | 682 | fire_power += power_change 683 | 684 | # display the fire_power 685 | power(fire_power) 686 | 687 | barrier(xlocation, randomHeight, barrier_width) 688 | 689 | # add ground 690 | gameDisplay.fill(green, rect=[0, display_height - ground_height, display_width, ground_height]) 691 | pygame.display.update() 692 | clock.tick(FPS) 693 | 694 | damage = e_fireShell(enemy_gun, enemyTankX, enemyTankY, 8, 50, xlocation, barrier_width, randomHeight,mainTankX, mainTankY ) 695 | player_health -= damage 696 | 697 | elif event.key == pygame.K_a: 698 | power_change = -1 699 | elif event.key == pygame.K_d: 700 | power_change = 1 701 | 702 | elif event.type == pygame.KEYUP: 703 | if event.key == pygame.K_LEFT or event.key == pygame.K_RIGHT: 704 | tankMove = 0 705 | 706 | if event.key == pygame.K_UP or event.key == pygame.K_DOWN: 707 | changeTur = 0 708 | 709 | if event.key == pygame.K_a or event.key == pygame.K_d: 710 | power_change = 0 711 | 712 | mainTankX += tankMove 713 | 714 | currentTurPos += changeTur 715 | 716 | # current turret possition should be between 0 and 8 717 | if currentTurPos > 8: 718 | currentTurPos = 8 719 | elif currentTurPos < 0: 720 | currentTurPos = 0 721 | 722 | if mainTankX - (tankWidth/2) < xlocation + barrier_width: 723 | mainTankX += 5 724 | 725 | 726 | gameDisplay.fill(white) 727 | health_bars(player_health, enemy_health) 728 | gun = tank(mainTankX, mainTankY, currentTurPos) 729 | enemy_gun = enemy_tank(enemyTankX, enemyTankY, 8) 730 | 731 | fire_power += power_change 732 | 733 | if fire_power > 100: 734 | fire_power = 100 735 | elif fire_power < 1: 736 | fire_power = 1 737 | 738 | # display the fire_power 739 | power(fire_power) 740 | 741 | barrier(xlocation, randomHeight, barrier_width) 742 | 743 | # add ground 744 | gameDisplay.fill(green, rect=[0, display_height - ground_height, display_width, ground_height]) 745 | 746 | pygame.display.update() 747 | 748 | if player_health < 1: 749 | game_over() 750 | elif enemy_health < 1: 751 | you_win() 752 | 753 | clock.tick(FPS) 754 | 755 | pygame.quit() 756 | quit() 757 | 758 | game_intro() 759 | gameLoop() --------------------------------------------------------------------------------