Untitled

import serial
import time
import curses
import math

# Konfiguracja portu szeregowego
SERIAL_PORT = '/dev/ttyACM0'  # Sprawdź port Arduino za pomocą `ls /dev/tty*`
BAUD_RATE = 115200  # Prędkość transmisji szeregowej

# Inicjalizacja połączenia szeregowego z Arduino
arduino = serial.Serial(SERIAL_PORT, BAUD_RATE, timeout=1)
time.sleep(2)  # Czekaj na inicjalizację Arduino

# Prędkości początkowe i zakres
current_speed1 = 0
current_speed2 = 0
MAX_SPEED = 255  # Maksymalna prędkość
ACCELERATION_TIME = 0.5  # Czas w sekundach do osiągnięcia pełnej prędkości
BRAKING_TIME = 0.2  # Czas w sekundach do pełnego zatrzymania
STEP_TIME = 0.05  # Odstęp czasu między krokami

def calculate_speed(v_start, v_target, elapsed_time, duration):
    """
    Oblicz aktualną prędkość na podstawie interpolacji kwadratowej.
    - v_start: prędkość początkowa.
    - v_target: prędkość docelowa.
    - elapsed_time: czas, jaki upłynął od rozpoczęcia zmiany prędkości.
    - duration: czas całkowity, w którym ma nastąpić zmiana prędkości.
    """
    if elapsed_time >= duration:
        return v_target
    t = elapsed_time / duration
    return v_start + (v_target - v_start) * (t ** 2)

def send_motor_commands(speed1, speed2):
    """
    Wyślij komendy do Arduino przez port szeregowy.
    - speed1, speed2: -255 do 255 (prędkości silników, znak określa kierunek)
    """
    try:
        message = f"{int(speed1)},{int(speed2)}\n"
        arduino.write(message.encode())
        print(f"Wysłano dane: {message.strip()}")
    except Exception as e:
        print(f"Błąd komunikacji: {e}")

def main(stdscr):
    """
    Główna pętla sterowania w trybie curses.
    - stdscr: ekran curses.
    """
    # Konfiguracja curses
    curses.cbreak()
    stdscr.nodelay(True)
    stdscr.keypad(True)
    stdscr.clear()
    stdscr.addstr(0, 0, "Sterowanie robotem za pomocą strzałek:")
    stdscr.addstr(1, 0, "↑: Do przodu, ↓: Do tyłu, ←: W lewo, →: W prawo, Spacja: Stop")
    stdscr.addstr(2, 0, "Naciśnij 'q', aby zakończyć program.")

    global current_speed1, current_speed2
    target_speed1, target_speed2 = 0, 0
    last_update = time.time()

    try:
        while True:
            key = stdscr.getch()
            now = time.time()
            elapsed_time = now - last_update

            if key == curses.KEY_UP:  # Strzałka w górę
                target_speed1, target_speed2 = MAX_SPEED, MAX_SPEED

            elif key == curses.KEY_DOWN:  # Strzałka w dół
                target_speed1, target_speed2 = -MAX_SPEED, -MAX_SPEED

            elif key == curses.KEY_LEFT:  # Strzałka w lewo
                target_speed1, target_speed2 = -MAX_SPEED // 2, MAX_SPEED // 2

            elif key == curses.KEY_RIGHT:  # Strzałka w prawo
                target_speed1, target_speed2 = MAX_SPEED // 2, -MAX_SPEED // 2

            elif key == ord(' '):  # Spacja - szybkie zatrzymanie
                target_speed1, target_speed2 = 0, 0

            elif key == ord('q'):  # Klawisz 'q' - zakończenie programu
                break

            # Oblicz nową prędkość
            current_speed1 = calculate_speed(
                current_speed1, target_speed1, elapsed_time, 
                ACCELERATION_TIME if target_speed1 > current_speed1 else BRAKING_TIME
            )
            current_speed2 = calculate_speed(
                current_speed2, target_speed2, elapsed_time, 
                ACCELERATION_TIME if target_speed2 > current_speed2 else BRAKING_TIME
            )

            # Wyślij nowe prędkości do Arduino
            send_motor_commands(current_speed1, current_speed2)

            # Resetuj czas aktualizacji
            last_update = now

            # Krótkie opóźnienie dla płynności
            time.sleep(STEP_TIME)

    except KeyboardInterrupt:
        pass
    finally:
        # Zatrzymanie silników na koniec programu
        send_motor_commands(0, 0)
        stdscr.addstr(4, 0, "Program zakończony. Silniki zatrzymane.")
        stdscr.refresh()
        time.sleep(1)

if __name__ == "__main__":
    curses.wrapper(main)