Select Git revision
math-parite.py
Forked from
Vuillemot Romain / INF-TC1
Source project has a limited visibility.
game.py 8.53 KiB
import pygame
from track import *
from car_drawer import CarDrawer
from car_model import Car
from input_providers import *
from car_data_display import CarDataDisplay
from pygame.math import Vector2
import json
from shapely.geometry import Point
from random import randint
class Game:
def __init__(self, width, height):
pygame.init()
pygame.display.set_caption("Car model")
self.window_width = width
self.window_height = height
self.screen = pygame.display.set_mode((width, height))
self.fps = 1
self.background = Background('BlueCheckerPatternPaper.png', [-500, -500])
self.clock = pygame.time.Clock()
self.ticks = 20
self.exit = False
self.x, self.y = [], []
self.time = []
def run(self):
car = Car(self.window_width / 20, self.window_height / 20)
track = Track('track3.svg')
track_drawer = TrackDrawer(track)
car.position.x, car.position.y = track.full_path[0][0] / 10 - 1366 / 20, track.full_path[0][1] / 10 - 768 / 20
car_drawer = CarDrawer()
input_provider = JoystickInputProvider()
if not input_provider.joysticks:
input_provider = KeyboardInputProvider()
car_data_display = CarDataDisplay(car)
trace = [(car.position.x * 10 + 1366/2, car.position.y * 10 + 768/2) for _ in range(3)]
while not self.exit:
dt = self.clock.tick(self.fps) / 1000
# Event queue
for event in pygame.event.get():
if event.type == pygame.QUIT:
self.exit = True
trace.pop(2)
trace.insert(0, (car.position.x * 10 + 1366/2, car.position.y * 10 + 768/2))
# User input
car_input = input_provider.get_input()
car.get_driver_input(car_input[0], car_input[1], car_input[2], car_input[3])
car.update(dt)
# Drawing
self.screen.fill((0, 0, 0))
track_drawer.draw(self.screen, car.position * 10, trace)
car_drawer.draw(self.screen, car)
car_data_display.display_data(self.screen)
pygame.display.flip()
pygame.quit()
def run_pid_controller(self, track_path, solution_path='solutionOpt.csv', dt=10):
car = Car(self.window_width / 20, self.window_height / 20, angle=90)
track = Track(track_path)
track_drawer = TrackDrawer(track)
solution = pd.read_csv(solution_path, index_col=0)
track.apply_deformations(list(solution.Deformation))
car.position.x, car.position.y = track.full_path[0][0] / 10 - 1366 / 20, track.full_path[0][1] / 10 - 768 / 20
car_drawer = CarDrawer()
car_data_display = CarDataDisplay(car, track)
input_provider = AutonomousDriver(solution)
time = 0
trace = [(car.position.x * 10 + 1366/2, car.position.y * 10 + 768/2) for _ in range(3)]
last_u = -1
while not self.exit:
"""fps perturbation"""
fps = 20
u_dt = 1/fps
# Handling time
self.clock.tick(1 / dt)
time += dt
# Event queue
for event in pygame.event.get():
if event.type == pygame.QUIT:
self.exit = True
# Input from solution file and PID controller
input_provider.index = track_drawer.chunk_indexes[-1] # Incrementing row index of solution matrix
car_input = input_provider.get_input(track.track_phase)
turning=car_input[3]
"""perturbations"""
incert_turning = 0 #5°
turning += randint(-incert_turning, incert_turning)
"""limit turning"""
max_steering = 30
if turning<0:
turning = max(turning, -max_steering)
else:
turning= min(turning, max_steering)
if abs(time-last_u)>u_dt:
last_u=time
car.get_driver_input(car_input[0], car_input[1], car_input[2], turning)
car.update(dt)
# Calculating position of a front center of a car
vector = Vector2(40, 0).rotate(-car.angle)
vector = np.array((vector.x + 1366 / 2, vector.y + 768 / 2))
front_center = Point(np.array((car.position.x * 10, car.position.y * 10)) + vector)
"""perturbations"""
R_simu = 150
R_reel = 8
incert_reel = 0 #0.2m
incert_simu = incert_reel*R_simu/R_reel
incert_simu = int(incert_simu)
error_x = randint(-incert_simu, incert_simu)
error_y = randint(-incert_simu, incert_simu)
error_point = Point(front_center.x+error_x, front_center.y+error_y)
# Calculating line error as a distance from front center to a given line (path)
i = 1# check if mid is past, if yes, track is on the left so we need to adapt
if track.is_starting or track.is_ending:
input_provider.line_error = track.mid_track-error_point.x
else:
if not track.is_right:
i=-1
e = LineString(track.visible_path).distance(error_point)
#print(e)
if Polygon(track.visible_path).contains(error_point):
input_provider.line_error = - i * e
else:
input_provider.line_error = i * e
self.x.append(front_center.x)
self.y.append(front_center.y)
self.time.append(time)
# Updating trace
trace.pop(2)
trace.insert(0, (car.position.x * 10 + 1366 / 2, car.position.y * 10 + 768 / 2))
# Drawing
self.screen.fill((0, 0, 0))
self.background.set_location([- car.position.x * 10, 768 - car.position.y * 10])
self.screen.blit(self.background.image, self.background.rect)
self.background.set_location([1200 - car.position.x * 10, 768 - car.position.y * 10])
self.screen.blit(self.background.image, self.background.rect)
self.background.set_location([2400 - car.position.x * 10, 768 - car.position.y * 10])
self.screen.blit(self.background.image, self.background.rect)
self.background.set_location([- car.position.x * 10, 768 + 1200 - car.position.y * 10])
self.screen.blit(self.background.image, self.background.rect)
self.background.set_location([1200 - car.position.x * 10, 768 + 1200 - car.position.y * 10])
self.screen.blit(self.background.image, self.background.rect)
self.background.set_location([2400 - car.position.x * 10, 768 + 1200 - car.position.y * 10])
self.screen.blit(self.background.image, self.background.rect)
self.background.set_location([- car.position.x * 10, 768 + 2400 - car.position.y * 10])
self.screen.blit(self.background.image, self.background.rect)
self.background.set_location([1200 - car.position.x * 10, 768 + 2400 - car.position.y * 10])
self.screen.blit(self.background.image, self.background.rect)
self.background.set_location([2400 - car.position.x * 10, 768 + 2400 - car.position.y * 10])
self.screen.blit(self.background.image, self.background.rect)
track_drawer.draw(self.screen, car.position * 10, trace)
car_drawer.draw(self.screen, car)
car_data_display.display_data(self.screen)
rect = pygame.Rect(front_center.x - car.position.x * 10, front_center.y - car.position.y * 10, 5, 5)
pygame.draw.rect(self.screen, (0, 0, 255), rect)
pygame.display.flip()
#check if track is done
completed = track.max_y<front_center.y
if completed:
break
# Checking if car has passed finishing-line
if input_provider.index == len(track.track_chunks) - 1:
break
# Checking if car completed track
# completed = True
# for chunk in track.track_chunks:
# if not chunk.is_active:
# completed = False
# break
print("Time:", time, "Completed:", completed)
data = {"time":self.time, "x":self.x, "y":self.y}
"""saving data"""
"""
with open('error.json', 'w') as outfile:
json.dump(data, outfile, indent=4)
print('SAVED')
"""
pygame.quit()