import pygame import random import time from math import * from copy import deepcopy display_width = 800 display_height = 600 world_size = display_width red = (200, 0, 0) blue = (0, 0, 255) green = (0, 155, 0) yellow = (200, 200, 0) white = (255, 255, 255) black = (0, 0, 0) grey = (67, 70, 75) car_length = 80.0 car_width = 60.0 wheel_length = 20 wheel_width = 6 max_steering_angle = pi/2 car_img = pygame.image.load("car60_40.png") track_img= pygame.image.load("racetrack2.png") def remap(value, maxInput, minInput, maxOutput, minOutput): value = maxInput if value > maxInput else value value = minInput if value < minInput else value inputSpan = maxInput - minInput outputSpan = maxOutput - minOutput scaledThrust = float(value - minInput) / float(inputSpan) return minOutput + (scaledThrust * outputSpan) origin = (display_width/2, display_height/2) history_points = [] pygame.init() screen = pygame.display.set_mode((display_width, display_height)) pygame.display.set_caption("Moving robot") clock = pygame.time.Clock() screen.fill(white) class Pallet: def __init__(self, x, y, rot) -> None: self.x = x self.y = y self.orientation = rot def draw(self): draw_rect([self.x, self.y], [[self.x+35, self.y+35], [self.x+35, self.y-35], [self.x-35, self.y-35], [self.x-35, self.y+35]], self.orientation, red) def draw_history(): for pt in history_points: pygame.draw.circle(screen, red, (pt[0], pt[1]), 1) class robot: def __init__(self): self.x = 0 self.y = 0 self.orientation = 0 self.steering_angle = 0.0 def set(self, x, y, orientation, steering_angle): if x >= world_size or x < 0: raise ValueError#, 'X coordinate out of bound' if y >= world_size or y < 0: raise ValueError#, 'Y coordinate out of bound' if orientation >= 2*pi or orientation < 0: raise ValueError#, 'Orientation must be in [0..2pi]' if abs(steering_angle) > max_steering_angle: raise ValueError#, 'Exceeding max steering angle' self.x = x self.y = y self.orientation = orientation self.steering_angle = steering_angle def move(self, turn, forward): theta = self.orientation # initial orientation alpha = turn # steering angle dist = forward # distance to be moved length = car_length # length of the robot if alpha > max_steering_angle: alpha = max_steering_angle if alpha < -max_steering_angle: alpha = -max_steering_angle # if dist < 0.0: # raise ValueError#, 'Moving backwards is not valid' # in local coordinates of robot beta = (dist/length)*tan(alpha) # turning angle # print degrees(beta) _x = _y = _theta = 0.0 if beta > 0.001 or beta < -0.001: radius = dist/beta # turning radius cx = self.x - sin(theta)*radius # center of the circle cy = self.y - cos(theta)*radius # center of the circle # Uncomment to see the center of the circle the robot is going about # pygame.draw.circle(screen, red, (int(cx), int(cy)), 5) # pygame.display.update() # in global coordinates of robot _x = cx + sin(theta + beta)*radius _y = cy + cos(theta + beta)*radius _theta = (theta + beta)%(2*pi) else: # straight motion _x = self.x + dist*cos(theta) _y = self.y - dist*sin(theta) _theta = (theta + beta)%(2*pi) self.x = _x self.y = _y self.orientation = _theta self.steering_angle = alpha self.x %= world_size self.y %= world_size def draw_rect(center, corners, rotation_angle, color): c_x = center[0] c_y = center[1] delta_angle = rotation_angle rotated_corners = [] for p in corners: temp = [] length = sqrt((p[0] - c_x)**2 + (c_y - p[1])**2) angle = atan2(c_y - p[1], p[0] - c_x) angle += delta_angle temp.append(c_x + length*cos(angle)) temp.append(c_y - length*sin(angle)) rotated_corners.append(temp) # draw rectangular polygon --> car body _ = pygame.draw.polygon(screen, color, (rotated_corners[0],rotated_corners[1],rotated_corners[2],rotated_corners[3])) def draw_robot(robot): car_x = robot.x car_y = robot.y orientation = robot.orientation steering_angle = robot.steering_angle p1 = [car_x-car_length/4,car_y-car_width/2] p2 = [car_x+(0.75*car_length),car_y-car_width/2] p3 = [car_x+(0.75*car_length),car_y+car_width/2] p4 = [car_x-car_length/4,car_y+car_width/2] # car body draw_rect([car_x, car_y], [p1, p2, p3, p4], orientation, yellow) # heading direction # h = [car_x+car_length/2,car_y] # length = car_length/2 # angle = atan2(car_y - h[1], h[0] - car_x) # angle += orientation # h[0] = car_x + length*cos(angle) # h[1] = car_y - length*sin(angle) # wheels # rotate center of wheel1 w1_c_x = car_x w1_c_y = car_y - car_width/3 length = sqrt((w1_c_x - car_x)**2 + (car_y - w1_c_y)**2) angle = atan2(car_y - w1_c_y, w1_c_x - car_x) angle += orientation w1_c_x = car_x + length*cos(angle) w1_c_y = car_y - length*sin(angle) # draw corners of wheel1 w1_p1 = [w1_c_x-wheel_length/2, w1_c_y-wheel_width/2] w1_p2 = [w1_c_x+wheel_length/2, w1_c_y-wheel_width/2] w1_p3 = [w1_c_x+wheel_length/2, w1_c_y+wheel_width/2] w1_p4 = [w1_c_x-wheel_length/2, w1_c_y+wheel_width/2] draw_rect([w1_c_x, w1_c_y], [w1_p1, w1_p2, w1_p3, w1_p4], orientation, black) w2_c_x = car_x + car_length/2 w2_c_y = car_y - car_width/3 length = sqrt((w2_c_x - car_x)**2 + (car_y - w2_c_y)**2) angle = atan2(car_y - w2_c_y, w2_c_x - car_x) angle += orientation w2_c_x = car_x + length*cos(angle) w2_c_y = car_y - length*sin(angle) w2_p1 = [w2_c_x-wheel_length/2, w2_c_y-wheel_width/2] w2_p2 = [w2_c_x+wheel_length/2, w2_c_y-wheel_width/2] w2_p3 = [w2_c_x+wheel_length/2, w2_c_y+wheel_width/2] w2_p4 = [w2_c_x-wheel_length/2, w2_c_y+wheel_width/2] draw_rect([w2_c_x, w2_c_y], [w2_p1, w2_p2, w2_p3, w2_p4], steering_angle + orientation, black) # rect = pygame.draw.polygon(screen, black, (w2_p1,w2_p2,w2_p3,w2_p4)) w3_c_x = car_x + car_length/2 w3_c_y = car_y + car_width/3 length = sqrt((w3_c_x - car_x)**2 + (car_y - w3_c_y)**2) angle = atan2(car_y - w3_c_y, w3_c_x - car_x) angle += orientation w3_c_x = car_x + length*cos(angle) w3_c_y = car_y - length*sin(angle) w3_p1 = [w3_c_x-wheel_length/2, w3_c_y-wheel_width/2] w3_p2 = [w3_c_x+wheel_length/2, w3_c_y-wheel_width/2] w3_p3 = [w3_c_x+wheel_length/2, w3_c_y+wheel_width/2] w3_p4 = [w3_c_x-wheel_length/2, w3_c_y+wheel_width/2] draw_rect([w3_c_x, w3_c_y], [w3_p1, w3_p2, w3_p3, w3_p4], steering_angle + orientation, black) # rect = pygame.draw.polygon(screen, black, (w3_p1,w3_p2,w3_p3,w3_p4)) w4_c_x = car_x w4_c_y = car_y + car_width/3 length = sqrt((w4_c_x - car_x)**2 + (car_y - w4_c_y)**2) angle = atan2(car_y - w4_c_y, w4_c_x - car_x) angle += orientation w4_c_x = car_x + length*cos(angle) w4_c_y = car_y - length*sin(angle) w4_p1 = [w4_c_x-wheel_length/2, w4_c_y-wheel_width/2] w4_p2 = [w4_c_x+wheel_length/2, w4_c_y-wheel_width/2] w4_p3 = [w4_c_x+wheel_length/2, w4_c_y+wheel_width/2] w4_p4 = [w4_c_x-wheel_length/2, w4_c_y+wheel_width/2] draw_rect([w4_c_x, w4_c_y], [w4_p1, w4_p2, w4_p3, w4_p4], orientation, black) # pygame.draw.line(screen, red, (h[0], h[1]),(int(car_x), int(car_y)), 1) # draw axle pygame.draw.line(screen, black, (w1_c_x, w1_c_y),(w4_c_x, w4_c_y), 1) # draw mid of axle pygame.draw.circle(screen, red, (int(car_x), int(car_y)), 3) def get_err(robot, pallet): r_x = robot.x r_y = robot.y r_r = degrees(robot.orientation) p_x = pallet.x p_y = pallet.y p_r = degrees(pallet.orientation) delta_y = r_y - p_y delta_rot = r_r - p_r return (delta_y, delta_rot) # def line_to_circle(input): # if input > 360: # input = input % 360 # if input > 180: # input -= 360 # if input < 360: # input = input % 360 # if input < -180: # input += 360 # return input def steering_law(dy, dt): ky = -110 kt = 5 angle_element = kt * dt linear_element= ky * atan(radians(dy)) delta = linear_element + angle_element # delta = line_to_circle(delta) return delta, (linear_element, angle_element) robot = robot() # robot.set_noise(0.1, 0.01, 5.0) orientation = 180.0 steering_angle = 0.0 #in radians robot.set(100, 400,radians(orientation), steering_angle) pallet = Pallet(500, 300, radians(190)) # pallet = Pallet(500, 300, radians(200)) exit = False delta_forward = 0.0 delta_steer = 0.0 while exit == False: screen.fill(black) # screen.blit(track_img, (0, 100)) # Uncomment following to see the exact racetrack #draw_track(250, 400, 200, grey) # draw_path(path, red) draw_robot(robot) pallet.draw() history_points.append((robot.x, robot.y)) draw_history() # pygame.draw.line(screen, green, (display_width/2, 0), (display_width/2, display_height), 1) # pygame.draw.line(screen, black, (0, display_height/2), (display_width, display_height/2), 1) pygame.display.update() clock.tick(100) err_y, err_t = get_err(robot, pallet) steering, debug = steering_law(err_y, err_t) delta_PWM = remap(steering, 180, -180, 60, -60) delta_forward = -1 # print("dy: {:.2f}, dt: {:.2f}, strng_raw: {:.2f}, strng_adjust: {:.2f}".format(0, 0, steering, 0)) print("dy: {:.2f}, dt: {:.2f}, strng_raw: {:.2f}, strng_adjust: {:.2f}, linear: {:.2f}, angular: {:.2f}".format(err_y, err_t, steering, delta_PWM, debug[0], debug[1])) delta_steer = radians(delta_PWM) # next = input("Continue?: ") # for event in pygame.event.get(): # if event.type == pygame.QUIT: # exit = True # if event.type == pygame.KEYDOWN: # if event.key == pygame.K_LEFT: # delta_steer = -radians(60) # elif event.key == pygame.K_RIGHT: # delta_steer = radians(60) # elif event.key == pygame.K_UP: # delta_forward = -2.0 # elif event.key == pygame.K_DOWN: # delta_forward = 2.0 # elif event.type == pygame.KEYUP: # if event.key == pygame.K_RIGHT or event.key == pygame.K_LEFT: # delta_steer = 0.0 # if event.key == pygame.K_UP or event.key == pygame.K_DOWN: # delta_forward = 0.0 robot.move(delta_steer, delta_forward)