Radar - Code

From RoboWiki
Jump to: navigation, search

Return back to project page: Radar - Jakub Vojtek

Python code for the Radar project:

from buildhat import Motor, DistanceSensor
import pygame
import time
import math


motor = Motor('D')
eyes = DistanceSensor('A')

def setup():
    pygame.init()
    pygame.display.set_caption('Radar')
    WIDTH, HEIGHT = 800, 800
    screen = pygame.display.set_mode((WIDTH, HEIGHT), pygame.SRCALPHA)

    background_image = pygame.image.load("pictures/radar_bg.png").convert()
    background_image = pygame.transform.scale(background_image, (WIDTH, HEIGHT))

    motor.start(2)

    return screen, background_image, WIDTH, HEIGHT


def draw_radar_arm(screen, center, angle, length):
    end_x = center[0] + length * math.cos(math.radians(angle))
    end_y = center[1] - length * math.sin(math.radians(angle))
    pygame.draw.line(screen, (0, 255, 0), center, (end_x, end_y), 2)
    pygame.draw.circle(screen, (0, 255, 0), (int(end_x), int(end_y)), 5)


def draw_shadow(screen, center, angle, distance, max_distance, arm_length):
    for dist in range(distance, max_distance + 1, 10):
        shadow_x = center[0] + (dist / max_distance) * arm_length * math.cos(math.radians(angle))
        shadow_y = center[1] - (dist / max_distance) * arm_length * math.sin(math.radians(angle))
        pygame.draw.circle(screen, (255, 102, 102, 128), (int(shadow_x), int(shadow_y)), 5)


def draw_text(screen, text, position, font, color=(255, 255, 255)):
    text_surface = font.render(text, True, color)
    screen.blit(text_surface, position)


def update_radar(screen, background_image, WIDTH, HEIGHT, max_distance=2000):
    center = (WIDTH // 2, HEIGHT // 2)
    arm_length = 345
    obstacle_positions = []

    closest_distance = float('inf')
    farthest_distance = float('-inf')

    angle = 0
    running = True
    font = pygame.font.Font(None, 25)

    while running:
        for event in pygame.event.get():
            if event.type == pygame.QUIT:
                running = False
            elif event.type == pygame.KEYDOWN:
                if event.key == pygame.K_LEFT:
                    max_distance = max(500, max_distance - 100)
                elif event.key == pygame.K_RIGHT:
                    max_distance = min(2000, max_distance + 100)

        screen.blit(background_image, (0, 0))

        angle = motor.get_position() % 360
        draw_radar_arm(screen, center, angle, arm_length)
        distance = eyes.get_distance()


        if distance > 0 and distance < max_distance:
            scaled_distance = (distance / max_distance) * arm_length
            obstacle_x = center[0] + scaled_distance * math.cos(math.radians(angle))
            obstacle_y = center[1] - scaled_distance * math.sin(math.radians(angle))
            obstacle_positions.append((obstacle_x, obstacle_y, distance, angle, time.time()))

            if distance < closest_distance:
                closest_distance = distance
            if distance > farthest_distance:
                farthest_distance = distance

        # draw obstacles and shadows
        current_time = time.time()
        for obstacle in obstacle_positions:
            if current_time - obstacle[4] < 2:
                draw_shadow(screen, center, obstacle[3], obstacle[2], max_distance, arm_length)
                pygame.draw.circle(screen, (255, 0, 0), (int(obstacle[0]), int(obstacle[1])), 5)
            else:
                obstacle_positions.remove(obstacle)

        draw_text(screen, f'Max View Distance: {max_distance}', (10, 10), font)
        draw_text(screen, f'Closest Object: {closest_distance}', (350, 10), font)
        draw_text(screen, f'Farthest Object: {farthest_distance}', (600, 10), font)

        pygame.display.flip()
        time.sleep(0.05)

    pygame.quit()


if __name__ == '__main__':
    screen, background_image, WIDTH, HEIGHT = setup()
    max_distance = 1000
    update_radar(screen, background_image, WIDTH, HEIGHT, max_distance)