Untitled

import pygame
import serial
import os
import time
from datetime import datetime

# Konfiguracja portu szeregowego
SERIAL_PORT = '/dev/ttyACM0'  # Sprawdź poprawny port (np. /dev/ttyUSB0)
BAUD_RATE = 115200  # Prędkość transmisji szeregowej
arduino = serial.Serial(SERIAL_PORT, BAUD_RATE, timeout=1)
time.sleep(2)  # Czekaj na inicjalizację Arduino

# Inicjalizacja Pygame
pygame.init()
screen = pygame.display.set_mode([240, 160])

# Konfiguracja kamery
from picamera import PiCamera
camera = PiCamera()
camera.resolution = (1280, 720)
camera.framerate = 25

record = 0  # Zmienna do śledzenia stanu nagrywania

# Prędkości i dynamika
MAX_SPEED = 255
ACCELERATION_STEP = 10  # Krok przyspieszania
BRAKING_STEP = 30       # Krok hamowania
current_speed_left = 0
current_speed_right = 0
target_speed_left = 0
target_speed_right = 0

def send_motor_commands(speed_left, speed_right):
    """
    Wysyła prędkości do Arduino przez port szeregowy.
    """
    try:
        message = f"{int(speed_left)},{int(speed_right)}\n"
        arduino.write(message.encode())
        print(f"Wysłano dane: {message.strip()}")
    except Exception as e:
        print(f"Błąd komunikacji z Arduino: {e}")

def adjust_speed():
    """
    Płynnie reguluje prędkość w kierunku docelowym.
    """
    global current_speed_left, current_speed_right
    # Silnik lewy
    if current_speed_left < target_speed_left:
        current_speed_left += ACCELERATION_STEP
        if current_speed_left > target_speed_left:
            current_speed_left = target_speed_left
    elif current_speed_left > target_speed_left:
        current_speed_left -= BRAKING_STEP
        if current_speed_left < target_speed_left:
            current_speed_left = target_speed_left

    # Silnik prawy
    if current_speed_right < target_speed_right:
        current_speed_right += ACCELERATION_STEP
        if current_speed_right > target_speed_right:
            current_speed_right = target_speed_right
    elif current_speed_right > target_speed_right:
        current_speed_right -= BRAKING_STEP
        if current_speed_right < target_speed_right:
            current_speed_right = target_speed_right

    # Wyślij zaktualizowane prędkości do Arduino
    send_motor_commands(current_speed_left, current_speed_right)

try:
    # Główna pętla programu
    running = True
    while running:
        for event in pygame.event.get():
            if event.type == pygame.KEYDOWN:
                if event.key == pygame.K_q:  # Wyjście z programu
                    running = False
                elif event.key == pygame.K_s:  # Wyłączenie systemu
                    os.system('sudo shutdown now')
                elif event.key == pygame.K_r:  # Rozpoczęcie nagrywania
                    if record == 0:
                        record = 1
                        moment = datetime.now()
                        camera.start_recording(f'/home/pi/Videos/vid_{moment.hour:02d}_{moment.minute:02d}_{moment.second:02d}.mjpg')
                elif event.key == pygame.K_t:  # Zakończenie nagrywania
                    if record == 1:
                        record = 0
                        camera.stop_recording()
                elif event.key == pygame.K_UP:  # Jazda do przodu
                    target_speed_left = MAX_SPEED
                    target_speed_right = MAX_SPEED
                elif event.key == pygame.K_DOWN:  # Jazda do tyłu
                    target_speed_left = -MAX_SPEED
                    target_speed_right = -MAX_SPEED
                elif event.key == pygame.K_LEFT:  # Skręt w lewo
                    target_speed_left = -MAX_SPEED // 2
                    target_speed_right = MAX_SPEED // 2
                elif event.key == pygame.K_RIGHT:  # Skręt w prawo
                    target_speed_left = MAX_SPEED // 2
                    target_speed_right = -MAX_SPEED // 2
                elif event.key == pygame.K_SPACE:  # Zatrzymanie
                    target_speed_left = 0
                    target_speed_right = 0

            elif event.type == pygame.KEYUP:
                # Zatrzymanie tylko po zwolnieniu klawiszy strzałek
                if event.key in [pygame.K_UP, pygame.K_DOWN, pygame.K_LEFT, pygame.K_RIGHT]:
                    target_speed_left = 0
                    target_speed_right = 0

        # Płynna regulacja prędkości
        adjust_speed()

        time.sleep(0.05)  # Krótki czas na aktualizację (20 Hz)

finally:
    # Zatrzymanie silników i sprzątanie
    send_motor_commands(0, 0)
    if record == 1:
        camera.stop_recording()
    arduino.close()
    print("Program zakończony.")