import pygame
import random
import numpy as np
import matplotlib.pyplot as plt
from matplotlib.animation import FuncAnimation
import serial
import os
import math
import time
from codrone_edu import *
#그래픽카드 사용 선언
os.environ["CUDA_DEVICE_ORDER"] = "PCI_BUS_ID"
os.environ["CUDA_VISIBLE_DEVICES"] = "1"
#아두이노 연결
ser = serial.Serial(port='COM7', baudrate=9600,
parity=serial.PARITY_NONE,
stopbits=serial.STOPBITS_ONE,
bytesize=serial.EIGHTBITS)
pygame.init()
width, height = 800, 800
radius = 360
w = 300
motion = 0
degree = 0
distance = 0
radarDist = 0
firstRun = 0
ser.x = 0
ser.y = 0
blip_countdown = 0
target = 0
fig, ax = plt.subplots(subplot_kw=dict(polar=True)) #polar 그리기
lines, = ax.plot([], [], lw=3, color='green')
def sigmoid(x):
return 1 / (1 + np.exp(-x))
# Generate x values
x_values = np.linspace(-7, 7, 100)
# 타겟을 보여준다
ser_x = radius + random.uniform(-0.05, 0.05) * radius * 5
ser_y = radius + random.uniform(-0.05, 0.05) * radius * 5
def init():
lines.set_data([], [])
return lines,
def update(frame):
degree = frame
ser_x = np.radians(degree)
ser_y = 1 # y축을 중심으로 데이터 계산
# lines.set_data(x, y)
lines.set_ydata(sigmoid(x_values - frame))
return lines,
# Pygame Setup
screen = pygame.display.set_mode((width, height))
pygame.display.set_caption('Radar Simulation')
clock = pygame.time.Clock()
def calculate_distance():
speed_of_light = 34
round_trip_time = 2.5
target_position = np.array([ser_x, ser_y])
radar_center = np.array([width/2, height/2])
#distance = np.linalg.norm(target_position - radar_center)
distance = speed_of_light * round_trip_time * (target_position - radar_center)
return distance,
def drawRadar():
screen.fill((0, 0, 0)) # 배경색
# 레이더 원을 그린다
for i in range(1, 4):
circle_radius = i * 105
circle_center = (radius, radius)
pygame.draw.circle(screen, (0, 205, 0), circle_center, circle_radius, 2)
font = pygame.font.SysFont("arial", 20)
text = font.render(str(i * 45), True, (0, 255, 0))
text_position = (radius + 10, radius - i * 95)
screen.blit(text, text_position)
# 회전하는 각도 360도 회전 후에 다시 1도로
if degree <= 359:
motion = 1
if degree >= 1:
motion = 0
pygame.draw.aaline(screen, (0, 250, 0), (radius, radius),
(radius + math.cos(math.radians(degree + 360)) * w+5,
radius + math.sin(math.radians(degree + 360)) * w+5), 7)
if motion == 0:
for i in range(60): #부채꼴 회전하는 선 60도
angle = math.radians(degree + 360 + i)
pygame.draw.aaline(screen, (0, 250, 0),
(radius, radius),
(radius + math.cos(angle) * w,
radius + math.sin(angle) * w), 9)
else:
for i in range(-20, 0):
angle = math.radians(degree + 360 + i)
color_value = max(0, 20 - (10 * abs(i)))
pygame.draw.aaline(screen, (0, color_value, 0),
(radius, radius),
(radius + math.cos(angle) * w,
radius + math.sin(angle) * w), 9)
for i in range(12):
angle = np.radians(30 * i)
line_start = (radius, radius)
line_end = (radius + np.cos(angle) * radius,
radius + np.sin(angle) * radius)
pygame.draw.line(screen, (0, 255, 0), line_start, line_end, 2)
font = pygame.font.SysFont("arial", 20)
text = font.render(str(i * 30), True, (0, 255, 0))
text_position = (line_end[0] + 5, line_end[1] - 10)
screen.blit(text, text_position)
# 부채꼴 효과
x_blip = np.radians(degree)
y_blip = 1
lines.set_data(x_blip, y_blip)
# 글을 쓴다
Distance = calculate_distance()
font = pygame.font.SysFont("arial", 20)
text_angle = font.render(f"Angle: {degree} deg", True, (0, 255, 0))
text_distance = font.render(f"Distance: {distance} cm", True, (0, 255, 0))
text_make = font.render(f"Radar Screen",True,(0,255,0))
text_auther = font.render(f"Product by Woo Yong Kim",True,(0,255,0))
screen.blit(text_angle, (100, 740))
screen.blit(text_distance, (100, 760))
screen.blit(text_make, (300,740))
screen.blit(text_auther, (300,770))
# Main loop로 실행 코드
running = True
round_number = 0 # Initialize round number
while running:
for event in pygame.event.get():
if event.type == pygame.QUIT:
running = False
# Update
degree += 1
if degree >= 360:
degree = 0
round_number += 1 # 회전각을 1씩 증가
# 그리기 실행
drawRadar()
calculate_distance()
#update(frame)
pygame.display.flip()
clock.tick(60)