융합_로보테크 AI 자율주행 로봇 개발자 과정-26/02/02

직진 50cm

import math, time, requests

BASE = "http://192.168.0.244:5000"

def get_pose():
    p = requests.get(f"{BASE}/control", timeout=1).json()["p"]
    return float(p["x"]), float(p["y"])

def cmd(lin, ang):
    requests.get(f"{BASE}/control", params={"lin": lin, "ang": ang}, timeout=1)

def main():
    x0, y0 = get_pose()
    px, py = x0, y0
    dist = 0.0

    try:
        while dist < 0.50:
            x, y = get_pose()
            dist += math.hypot(x - px, y - py)
            px, py = x, y
            cmd(0.15, 0.0)      # 멈추지 않고 계속 전진
            time.sleep(0.05)
    finally:
        cmd(0.0, 0.0)          # 50cm 도달(또는 예외) 후 정지

if __name__ == "__main__":
    main()

 

제자리 회전 360

import math, time, requests

BASE = "http://192.168.0.244:5000"

def get_yaw():
    return float(requests.get(f"{BASE}/control", timeout=1).json()["p"]["a"])

def cmd(lin, ang):
    requests.get(f"{BASE}/control", params={"lin": lin, "ang": ang}, timeout=1)

def wrap(a):
    return (a + math.pi) % (2 * math.pi) - math.pi

def main():
    prev = get_yaw()
    turned = 0.0

    try:
        while turned < 2 * math.pi:
            cmd(0.0, -0.6)          # 좌회전 (제자리)
            time.sleep(0.05)

            now = get_yaw()
            turned += abs(wrap(now - prev))
            prev = now
    finally:
        cmd(0.0, 0.0)               # 한 바퀴 후 정지

if __name__ == "__main__":
    main()

 

직선 주행 + 회전 (1m + 180도 회전)*2(원위치)

import json, math, time, requests
from dataclasses import dataclass
from enum import Enum, auto
from typing import Dict, Any

def clamp(v, lo, hi): return max(lo, min(hi, v))
def wrap(a): return (a + math.pi) % (2 * math.pi) - math.pi
def ts(): return time.time()

ANGLE_ALIGN_TOL = math.radians(6.0)
MIN_ANG_CMD = 0.18

@dataclass
class Pose:
    x: float
    y: float
    a: float

class State(Enum):
    FORWARD = auto()
    TURN1 = auto()
    GO_HOME = auto()
    TURN2 = auto()
    STOP = auto()

BASE = "http://192.168.0.243:5000"

def get_pose() -> Pose:
    p = requests.get(f"{BASE}/control", timeout=1).json()["p"]
    return Pose(float(p["x"]), float(p["y"]), float(p["a"]))

def cmd(lin: float, ang: float):
    requests.get(f"{BASE}/control",
                 params={"lin": f"{lin:.3f}", "ang": f"{ang:.3f}"},
                 timeout=1)

class JSONL:
    def __init__(self, path="turtlebot_run.jsonl"):
        self.path = path
    def write(self, obj: Dict[str, Any]):
        with open(self.path, "a", encoding="utf-8") as f:
            f.write(json.dumps(obj, ensure_ascii=False) + "\n")

class SimpleReturnController:
    def __init__(self):
        self.state = State.FORWARD
        self.start = None
        self.prev = None
        self.travel = 0.0
        self.turned = 0.0
        self.logger = JSONL()

        self.lin = 0.15
        self.ang = 0.65
        self.dt = 0.05

    def rel_pose(self, p: Pose) -> Pose:
        dx, dy = p.x - self.start.x, p.y - self.start.y
        c, s = math.cos(-self.start.a), math.sin(-self.start.a)
        return Pose(c*dx - s*dy, s*dx + c*dy, wrap(p.a - self.start.a))

    def step_forward(self, p: Pose):
        if self.prev:
            self.travel += math.hypot(p.x - self.prev.x, p.y - self.prev.y)
        self.prev = p

        if self.travel >= 1.0:
            cmd(0.0, 0.0)
            self.state = State.TURN1
            self.prev = None
            return

        cmd(self.lin, 0.0)

    def step_turn(self, p: Pose, target=math.pi):
        if self.prev:
            self.turned += abs(wrap(p.a - self.prev.a))
        self.prev = p

        if self.turned >= target:
            cmd(0.0, 0.0)
            self.turned = 0.0
            self.prev = None
            self.state = State.GO_HOME if self.state == State.TURN1 else State.STOP
            return

        cmd(0.0, -self.ang)

    def step_go_home(self, pr: Pose):
        d = math.hypot(pr.x, pr.y)
        if d < 0.10:
            cmd(0.0, 0.0)
            self.state = State.TURN2
            return

        bearing = math.atan2(-pr.y, -pr.x)
        e = wrap(bearing - pr.a)

        if abs(e) < ANGLE_ALIGN_TOL:
            ang = 0.0
        else:
            ang = clamp(2.0 * e, -0.6, 0.6)
            if abs(ang) < MIN_ANG_CMD:
                ang = MIN_ANG_CMD if ang > 0 else -MIN_ANG_CMD

        lin = 0.08 if d < 0.4 else self.lin
        cmd(lin, ang)

    def run(self):
        print("Start")
        try:
            while self.state != State.STOP:
                p = get_pose()
                if self.start is None:
                    self.start = p

                pr = self.rel_pose(p)

                if self.state == State.FORWARD:
                    self.step_forward(p)
                elif self.state == State.TURN1:
                    self.step_turn(p)
                elif self.state == State.GO_HOME:
                    self.step_go_home(pr)
                elif self.state == State.TURN2:
                    self.step_turn(p)

                self.logger.write({
                    "t": ts(),
                    "state": self.state.name,
                    "p_world": {"x": p.x, "y": p.y, "a": p.a},
                    "p_rel": {"x": pr.x, "y": pr.y, "a": pr.a},
                    "travel": self.travel,
                    "turned": self.turned
                })

                time.sleep(self.dt)

            cmd(0.0, 0.0)
            print("STOP")

        except KeyboardInterrupt:
            cmd(0.0, 0.0)
            print("Interrupted")

if __name__ == "__main__":
    SimpleReturnController().run()

GUI(tkinter)에서 지정한 좌표까지 갔다가 복귀하여 정렬

import math
import tkinter as tk
from tkinter import ttk, messagebox
from goto_single_waypoint import GoToSingleWaypoint


class SettingPage(ttk.Frame):
    def __init__(self, master):
        super().__init__(master)
        # 설정값
        self.view_width = 400
        self.view_height = 200
        self.base_width = 200
        self.base_height = 100

        self.waypoints = []  # 실제 이동할 좌표들 (로봇 기준)
        self.ui_points = []  # 캔버스 상의 좌표들 (UI 기준)

        # 시작 지점 설정 (로봇 기준 50, 50)
        self.start_robot_x = 50
        self.start_robot_y = 50
        # 시작 지점을 UI 좌표로 변환
        self.start_ui_x = self.start_robot_x * (self.view_width / self.base_width)
        self.start_ui_y = self.start_robot_y * (self.view_height / self.base_height)

        # UI 구성
        self.setup_ui()
        self.draw_grid()

        # 시작 위치 표시
        self.draw_start_marker()

    def setup_ui(self):
        self.side_panel = tk.Frame(self, width=200, padx=10, pady=10)
        self.side_panel.pack(side='left', fill='y')

        btn_opts = {'width': 18, 'height': 3, 'bd': 4, 'relief': 'ridge'}

        self.follow_btn = tk.Button(self.side_panel, text='좌표탐색\n(첫번째 점)',
                                    state='disabled', command=self.on_follow_first_point, **btn_opts)
        self.follow_btn.pack(padx=5, pady=2)

        self.waypoints_btn = tk.Button(self.side_panel, text='경로따라가기\n(점 2개 이상)',
                                       state='disabled', **btn_opts)
        self.waypoints_btn.pack(padx=5, pady=2)

        # 최단 경로 버튼에 command 연결
        self.dijkstra_btn = tk.Button(self.side_panel, text='최단 경로로 바꾸기\n(점 2개 이상)',
                                      state='disabled', command=self.dijkstra, **btn_opts)
        self.dijkstra_btn.pack(padx=5, pady=2)

        tk.Label(self.side_panel, text="\n[좌표 목록]", font=("Arial", 10, "bold")).pack()
        self.pos_listbox = tk.Listbox(self.side_panel, width=22, height=5, font=("Consolas", 9))
        self.pos_listbox.pack(pady=5)

        self.reset_btn = tk.Button(self.side_panel, text='좌표 초기화', command=self.reset_points)
        self.reset_btn.pack(side='bottom', pady=10)

        self.main_panel = tk.Frame(self, pady=10)
        self.main_panel.pack(side='right', fill='both', expand=True)
        self.info_label = tk.Label(self.main_panel, text='캔버스에 좌표를 찍어주세요 (최대 5개)')
        self.info_label.pack(pady=5)

        self.canvas = tk.Canvas(self.main_panel, width=self.view_width, height=self.view_height,
                                bg='white', highlightthickness=1, highlightbackground="black")
        self.canvas.pack(pady=(5, 10), padx=10)
        self.canvas.bind('<Button-1>', self.add_point)

    def on_follow_first_point(self):
        if not self.waypoints:
            return
        x, y = self.waypoints[0]
        GoToSingleWaypoint(x, y).run()

    def draw_start_marker(self):
        ux, uy = self.start_ui_x, self.start_ui_y
        self.canvas.create_rectangle(ux - 4, uy - 4, ux + 4, uy + 4, fill="black", tags="start_marker")
        self.canvas.create_text(ux, uy - 12, text="START", font=("Arial", 8, "bold"), tags="start_marker")

    def add_point(self, event):
        if len(self.waypoints) >= 5:
            messagebox.showinfo('알림', '최대 5개의 좌표만 설정할 수 있습니다.')
            return

        ui_x, ui_y = event.x, event.y
        robot_x = int(ui_x * (self.base_width / self.view_width))
        robot_y = int(ui_y * (self.base_height / self.view_height))

        # 선 긋기 로직
        if len(self.ui_points) == 0:
            prev_x, prev_y = self.start_ui_x, self.start_ui_y
        else:
            prev_x, prev_y = self.ui_points[-1]

        self.canvas.create_line(prev_x, prev_y, ui_x, ui_y, fill="gray", dash=(4, 4), tags="path_line")

        self.ui_points.append((ui_x, ui_y))
        self.save_and_draw(ui_x, ui_y, robot_x, robot_y, 'red')

    def save_and_draw(self, x, y, robot_x, robot_y, color):
        self.waypoints.append((robot_x, robot_y))
        self.canvas.create_oval(x - 3, y - 3, x + 3, y + 3, fill=color, tags='point')
        self.pos_listbox.insert(tk.END, f'({robot_x}, {robot_y})')

        if len(self.waypoints) >= 1:
            self.follow_btn.config(state='normal', bg='#e1f5fe')
        if len(self.waypoints) >= 2:
            self.waypoints_btn.config(state='normal', bg='#e1f5fe')
            self.dijkstra_btn.config(state='normal', bg='#e1f5fe')
        self.update_info()

    def reset_points(self):
        self.canvas.delete("point")
        self.canvas.delete("path_line")
        self.pos_listbox.delete(0, tk.END)
        self.waypoints.clear()
        self.ui_points.clear()
        self.follow_btn.config(state="disabled", bg="SystemButtonFace")
        self.waypoints_btn.config(state="disabled", bg="SystemButtonFace")
        self.dijkstra_btn.config(state="disabled", bg="SystemButtonFace")
        self.update_info()

    def draw_grid(self):
        for i in range(0, self.base_height + 1, 10):
            ui_y = i * (self.view_height / self.base_height)
            self.canvas.create_line(0, ui_y, self.view_width, ui_y, fill="#e0e0e0", tags="grid")
        for j in range(0, self.base_width + 1, 10):
            ui_x = j * (self.view_width / self.base_width)
            self.canvas.create_line(ui_x, 0, ui_x, self.view_height, fill="#e0e0e0", tags="grid")
        self.canvas.tag_lower("grid")

    def update_info(self):
        self.info_label.config(text=f"등록된 좌표: {len(self.waypoints)}/5개")

    def get_distance(self, p1, p2):
        """두 좌표 사이의 직선 거리를 계산 (피타고라스)"""
        return math.sqrt((p1[0] - p2[0]) ** 2 + (p1[1] - p2[1]) ** 2)

    def dijkstra(self):
        """
        순열(Permutations)을 이용해 모든 가능한 경로의 길이를 계산하고
        그 중 가장 짧은 '진짜 최단 경로'를 선택합니다.
        """
        import itertools

        if len(self.waypoints) < 2:
            return

        # 1. 가능한 모든 방문 순서 조합 생성 (점의 개수가 적어 완전 탐색 가능)
        # points_with_ui : [(robot_coord, ui_coord), ...]
        points_data = list(zip(self.waypoints, self.ui_points))
        all_permutations = list(itertools.permutations(points_data))

        best_path = None
        min_total_distance = float('inf')

        # 2. 각 경로의 총 거리 계산
        for path in all_permutations:
            current_dist = 0
            # 시작점(50, 50)에서 첫 번째 점까지의 거리
            start_pos = (self.start_robot_x, self.start_robot_y)
            current_dist += self.get_distance(start_pos, path[0][0])

            # 이후 점들 간의 거리 합산
            for i in range(len(path) - 1):
                current_dist += self.get_distance(path[i][0], path[i + 1][0])

            # 최단 거리 갱신
            if current_dist < min_total_distance:
                min_total_distance = current_dist
                best_path = path

        # 3. 최적의 경로로 데이터 업데이트
        self.waypoints = [item[0] for item in best_path]
        self.ui_points = [item[1] for item in best_path]

        self.redraw_canvas()
        messagebox.showinfo("최적화 완료", f"수학적 최단 경로를 찾았습니다.\n총 거리: {min_total_distance:.2f}")

    def redraw_canvas(self):
        """정렬된 데이터를 바탕으로 화면 갱신"""
        self.canvas.delete("point")
        self.canvas.delete("path_line")
        self.pos_listbox.delete(0, tk.END)

        prev_ux, prev_uy = self.start_ui_x, self.start_ui_y

        for i, (ux, uy) in enumerate(self.ui_points):
            # 선 다시 그리기 (파란색)
            self.canvas.create_line(prev_ux, prev_uy, ux, uy, fill="blue", width=2, tags="path_line")
            # 점 다시 그리기
            self.canvas.create_oval(ux - 4, uy - 4, ux + 4, uy + 4, fill="blue", tags='point')
            # 리스트박스 갱신
            rx, ry = self.waypoints[i]
            self.pos_listbox.insert(tk.END, f'{i + 1}번: ({rx}, {ry})')
            prev_ux, prev_uy = ux, uy


class MainWindow(tk.Tk):
    def __init__(self, *args, **kwargs):
        super().__init__(*args, **kwargs)
        self.title("ROBOT PATH CONTROL SYSTEM")
        self.geometry("650x450")
        self.setting_page = SettingPage(self)
        self.setting_page.pack(expand=True, fill='both')

    def run(self):
        self.mainloop()


if __name__ == '__main__':
    mw = MainWindow()
    mw.run()

import math, time, requests
from dataclasses import dataclass
from enum import Enum, auto

# ======================
# 유틸
# ======================
def wrap(a):
    return (a + math.pi) % (2 * math.pi) - math.pi

def clamp(v, lo, hi):
    return max(lo, min(hi, v))

def deg(rad):
    return math.degrees(rad)

# ======================
# 설정
# ======================
BASE = "http://192.168.0.243:5000"

GUI_START_X = 50  # cm
GUI_START_Y = 50  # cm
CM2M = 0.01

LIN = 0.15
ANG = 0.6
DT = 0.05

ANGLE_TOL = math.radians(3)
DIST_TOL = 0.03
STRAIGHT_KP = 0.5
STRAIGHT_ANG_MAX = 0.12

STOP_HOLD = 1.0

# ======================
# 데이터
# ======================
@dataclass
class Pose:
    x: float
    y: float
    a: float   # yaw (rad)

class State(Enum):
    TURN_TO_GOAL = auto()
    GO_TO_GOAL = auto()
    TURN_TO_HOME = auto()
    GO_HOME = auto()
    TURN_RECOVER = auto()
    STOP = auto()

# ======================
# 통신
# ======================
def get_pose() -> Pose:
    p = requests.get(f"{BASE}/control", timeout=1).json()["p"]
    return Pose(float(p["x"]), float(p["y"]), float(p["a"]))

def cmd(lin, ang):
    requests.get(
        f"{BASE}/control",
        params={"lin": f"{lin:.3f}", "ang": f"{ang:.3f}"},
        timeout=1
    )

# ======================
# 컨트롤러
# ======================
class GoToSingleWaypoint:
    def __init__(self, tx, ty):
        # GUI → 상대 이동 (m)
        self.dx = (tx - GUI_START_X) * CM2M
        self.dy = (ty - GUI_START_Y) * CM2M

        self.target_dist = math.hypot(self.dx, self.dy)
        self.target_angle = math.atan2(self.dy, self.dx)

        self.state = State.TURN_TO_GOAL
        self.start = None
        self.prev = None

        self.travel = 0.0
        self.turned = 0.0

        print("\n=== MISSION START ===")
        print(f"GUI target (cm) : ({tx}, {ty})")
        print(f"Relative (m)    : dx={self.dx:.3f}, dy={self.dy:.3f}")
        print(f"Target dist    : {self.target_dist:.3f} m")
        print(f"Target angle   : {deg(self.target_angle):.2f} deg")

    def run(self):
        try:
            while self.state != State.STOP:
                p = get_pose()

                if self.start is None:
                    self.start = p
                    self.prev = p
                    self.goal_yaw = wrap(p.a + self.target_angle)
                    print(f"[INIT] start yaw={deg(p.a):.2f} deg")
                    print(f"[INIT] goal yaw ={deg(self.goal_yaw):.2f} deg")
                    time.sleep(DT)
                    continue

                # 이동량 누적
                step = math.hypot(p.x - self.prev.x, p.y - self.prev.y)
                self.travel += step
                self.prev = p

                lin = 0.0
                ang = 0.0

                # -------- TURN TO GOAL --------
                if self.state == State.TURN_TO_GOAL:
                    err = wrap(self.goal_yaw - p.a)
                    if abs(err) < ANGLE_TOL:
                        self.state = State.GO_TO_GOAL
                        self.travel = 0.0
                        print("[STATE] GO_TO_GOAL")
                    else:
                        ang = clamp(2.0 * err, -ANG, ANG)

                # -------- GO TO GOAL --------
                elif self.state == State.GO_TO_GOAL:
                    if self.travel >= self.target_dist - DIST_TOL:
                        self.state = State.TURN_TO_HOME
                        self.turned = 0.0
                        self.prev = p
                        print("[STATE] TURN_TO_HOME")
                    else:
                        lin = LIN
                        err = wrap(self.goal_yaw - p.a)
                        ang = clamp(STRAIGHT_KP * err,
                                    -STRAIGHT_ANG_MAX,
                                    STRAIGHT_ANG_MAX)

                # -------- TURN TO HOME --------
                elif self.state == State.TURN_TO_HOME:
                    home_yaw = wrap(self.start.a + math.pi)
                    err = wrap(home_yaw - p.a)
                    if abs(err) < ANGLE_TOL:
                        self.state = State.GO_HOME
                        self.travel = 0.0
                        print("[STATE] GO_HOME")
                    else:
                        ang = clamp(2.0 * err, -ANG, ANG)

                # -------- GO HOME --------
                elif self.state == State.GO_HOME:
                    if self.travel >= self.target_dist - DIST_TOL:
                        self.state = State.TURN_RECOVER
                        print("[STATE] TURN_RECOVER")
                    else:
                        lin = LIN

                # -------- TURN RECOVER --------
                elif self.state == State.TURN_RECOVER:
                    err = wrap(self.start.a - p.a)
                    if abs(err) < ANGLE_TOL:
                        self.state = State.STOP
                        print("[STATE] STOP")
                    else:
                        ang = clamp(2.0 * err, -ANG, ANG)

                print(
                    f"[{self.state.name}] "
                    f"pose=({p.x:.2f},{p.y:.2f},{deg(p.a):.1f}deg) "
                    f"travel={self.travel:.2f} "
                    f"cmd=({lin:.2f},{deg(ang):.1f}deg/s)"
                )

                cmd(lin, ang)
                time.sleep(DT)

            # 정지 보장
            t0 = time.time()
            while time.time() - t0 < STOP_HOLD:
                cmd(0.0, 0.0)
                time.sleep(0.05)

            print("=== MISSION END ===\n")

        except KeyboardInterrupt:
            cmd(0.0, 0.0)

목적지까지 가고 복귀도 되지만 속도가 너무 느림

ㄴ움직일때마다 위치 파악 새로 하고, 경로 설정해서

 

연속제어로 변경해보기

import math, time, requests

# ======================
# 설정
# ======================
BASE = "http://192.168.0.243:5000"

GUI_START_X = 50
GUI_START_Y = 50
CM2M = 0.01

CTRL_HZ = 50
DT = 1.0 / CTRL_HZ

LIN_MAX = 0.22
ANG_MAX = 1.2

KP_TURN = 2.8
KP_STRAIGHT = 1.0

ANGLE_TOL = math.radians(2.0)
POS_TOL = 0.03
DEAD_ANGLE = math.radians(1.5)

# ======================
def wrap(a):
    return (a + math.pi) % (2 * math.pi) - math.pi

def clamp(v, lo, hi):
    return max(lo, min(hi, v))

def get_pose():
    p = requests.get(f"{BASE}/control", timeout=0.2).json()["p"]
    return float(p["x"]), float(p["y"]), float(p["a"])

def cmd(lin, ang):
    requests.get(
        f"{BASE}/control",
        params={"lin": f"{lin:.3f}", "ang": f"{ang:.3f}"},
        timeout=0.2
    )

# ======================
class GoToSingleWaypoint:
    def __init__(self, tx, ty):
        # 목표 (cm → m)
        self.dx = (tx - GUI_START_X) * CM2M
        self.dy = (ty - GUI_START_Y) * CM2M

        self.goal_dist = math.hypot(self.dx, self.dy)
        self.goal_angle = math.atan2(self.dy, self.dx)

        self.state = "TURN_TO_GOAL"
        self.start_pose = None
        self.running = True

        print("\n========== START ==========")
        print(f"[TARGET] GUI=({tx},{ty})  rel=({self.dx:.3f},{self.dy:.3f}) m")
        print(f"[TARGET] dist={self.goal_dist:.3f} m  angle={math.degrees(self.goal_angle):.2f} deg")

    def run(self):
        try:
            while self.running:
                x, y, yaw = get_pose()

                if self.start_pose is None:
                    self.start_pose = (x, y, yaw)
                    self.goal_yaw = wrap(yaw + self.goal_angle)
                    self.home_yaw = wrap(yaw + math.pi)
                    time.sleep(DT)
                    continue

                sx, sy, syaw = self.start_pose
                dx = x - sx
                dy = y - sy
                dist = math.hypot(dx, dy)

                lin = 0.0
                ang = 0.0

                # ======================
                if self.state == "TURN_TO_GOAL":
                    err = wrap(self.goal_yaw - yaw)
                    print(f"[TURN_TO_GOAL] err={math.degrees(err):.2f}")
                    if abs(err) < ANGLE_TOL:
                        self.state = "GO_TO_GOAL"
                    else:
                        ang = clamp(KP_TURN * err, -ANG_MAX, ANG_MAX)

                elif self.state == "GO_TO_GOAL":
                    print(f"[GO_TO_GOAL] dist={dist:.3f}/{self.goal_dist:.3f}")
                    if dist >= self.goal_dist - POS_TOL:
                        self.state = "TURN_TO_HOME"
                    else:
                        lin = LIN_MAX
                        err = wrap(self.goal_yaw - yaw)
                        if abs(err) > DEAD_ANGLE:
                            ang = clamp(KP_STRAIGHT * err, -0.4, 0.4)

                elif self.state == "TURN_TO_HOME":
                    err = wrap(self.home_yaw - yaw)
                    print(f"[TURN_TO_HOME] err={math.degrees(err):.2f}")
                    if abs(err) < ANGLE_TOL:
                        self.state = "GO_HOME"
                    else:
                        ang = clamp(KP_TURN * err, -ANG_MAX, ANG_MAX)

                elif self.state == "GO_HOME":
                    print(f"[GO_HOME] dist={dist:.3f}")
                    if dist <= POS_TOL:
                        self.state = "TURN_RECOVER"
                    else:
                        lin = LIN_MAX

                elif self.state == "TURN_RECOVER":
                    err = wrap(syaw - yaw)
                    print(f"[TURN_RECOVER] err={math.degrees(err):.2f}")
                    if abs(err) < ANGLE_TOL:
                        self.state = "STOP"
                    else:
                        ang = clamp(KP_TURN * err, -ANG_MAX, ANG_MAX)

                elif self.state == "STOP":
                    print("[STOP] parked")
                    cmd(0.0, 0.0)
                    return

                cmd(lin, ang)
                time.sleep(DT)

        except KeyboardInterrupt:
            cmd(0.0, 0.0)
            print("[INTERRUPTED]")