diff --git a/README.md b/README.md index 9bf6ab6..49bcefc 100644 --- a/README.md +++ b/README.md @@ -34,5 +34,3 @@ python3 ./main.py ### Known Bugs: -- Because the variables don't get transferred over yet, you must click on the button editor tab before exporting. -- The driver controller's movement stick is rotated by 90 degrees (Maybe) diff --git a/main.py b/main.py index 35c02f2..6fc2d69 100644 --- a/main.py +++ b/main.py @@ -2,7 +2,7 @@ import sys import os import numpy as np from PySide6.QtWidgets import QApplication, QLabel, QMainWindow, QPushButton, QVBoxLayout, QWidget -from PySide6.QtGui import QPixmap, QMouseEvent, QPainter, QPen, QColor, QPainterPath +from PySide6.QtGui import QPixmap, QMouseEvent, QPainter, QPen, QColor, QPainterPath, QPolygon, QFont from PySide6.QtCore import Qt, QPoint, QRect class MainWindow(QMainWindow): @@ -41,17 +41,21 @@ class MainWindow(QMainWindow): self.setMouseTracking(True) # Variables + self.last_click_pos = QPoint() + self.coordinates = np.empty((0, 2), dtype=int) + self.last_click_time = 0 self.robot_size = 35 self.handle_size = 15 self.rotation_handle_distance = 35 self.handles = [] self.rotation_handles = [] + self.robot_angles = [] self.dragging_handle = False - self.dragging_handle_index = -1 self.dragging_robot = False - self.dragging_robot_index = -1 self.dragging_rotation_handle = False + self.dragging_handle_index = -1 + self.dragging_robot_index = -1 self.dragging_rotation_handle_index = -1 def mousePressEvent(self, event: QMouseEvent): @@ -65,21 +69,41 @@ class MainWindow(QMainWindow): self.calculate_handle_pos() self.calculate_rotation_handle_pos() self.draw_scene() + self.draw_scene() elif event.button() == Qt.LeftButton: - handle_index = self.is_point_in_handle(x, y) - if handle_index != -1: - self.dragging_handle = True - self.dragging_handle_index = handle_index - else: + current_time = event.timestamp() + if (current_time - self.last_click_time < 300 and + (pos - self.last_click_pos).manhattanLength() < 5): + # Double click detected robot_index = self.is_point_in_robot(x, y) if robot_index != -1: - self.dragging_robot = True - self.dragging_robot_index = robot_index + self.delete_robot(robot_index) + else: + handle_index = self.is_point_in_handle(x, y) + if handle_index != -1: + self.dragging_handle = True + self.dragging_handle_index = handle_index else: - rotation_handle_index = self.is_point_in_rotation_handle(x, y) - if rotation_handle_index != -1: - self.dragging_rotation_handle = True - self.dragging_rotation_handle_index = rotation_handle_index + robot_index = self.is_point_in_robot(x, y) + if robot_index != -1: + self.dragging_robot = True + self.dragging_robot_index = robot_index + else: + rotation_handle_index = self.is_point_in_rotation_handle(x, y) + if rotation_handle_index != -1: + self.dragging_rotation_handle = True + self.dragging_rotation_handle_index = rotation_handle_index + + self.last_click_time = current_time + self.last_click_pos = pos + + def delete_robot(self, index): + self.coordinates = np.delete(self.coordinates, index, axis=0) + if index < len(self.handles): + del self.handles[index] + if index < len(self.rotation_handles): + del self.rotation_handles[index] + self.draw_scene() def mouseMoveEvent(self, event: QMouseEvent): if self.dragging_handle: @@ -104,11 +128,15 @@ class MainWindow(QMainWindow): dy = y - robot_y angle = np.arctan2(dy, dx) + # Update the robot angle + self.robot_angles[self.dragging_rotation_handle_index] = angle + # Update the position of the rotation handle based on the angle and distance rotation_handle_x = int(robot_x + self.rotation_handle_distance * np.cos(angle)) rotation_handle_y = int(robot_y + self.rotation_handle_distance * np.sin(angle)) self.rotation_handles[self.dragging_rotation_handle_index] = QPoint(rotation_handle_x, rotation_handle_y) self.draw_scene() + self.draw_scene() def mouseReleaseEvent(self, event: QMouseEvent): if event.button() == Qt.LeftButton: @@ -148,47 +176,52 @@ class MainWindow(QMainWindow): def calculate_rotation_handle_pos(self): for i, (x, y) in enumerate(self.coordinates): + if i >= len(self.robot_angles): + self.robot_angles.append(np.pi) # Default angle for new robots + + angle = self.robot_angles[i] + + rotation_handle_x = int(x + self.rotation_handle_distance * np.cos(angle)) + rotation_handle_y = int(y + self.rotation_handle_distance * np.sin(angle)) + if i >= len(self.rotation_handles): - angle = np.pi / 4 - rotation_handle_x = int(x + self.rotation_handle_distance * np.cos(angle)) - rotation_handle_y = int(y - self.rotation_handle_distance * np.sin(angle)) self.rotation_handles.append(QPoint(rotation_handle_x, rotation_handle_y)) else: - # Update the position of existing rotation handles - angle = np.pi / 4 - rotation_handle_x = int(x + self.rotation_handle_distance * np.cos(angle)) - rotation_handle_y = int(y - self.rotation_handle_distance * np.sin(angle)) - self.rotation_handles[i] = QPoint(rotation_handle_x, rotation_handle_y) + self.rotation_handles[i] = QPoint(rotation_handle_x, rotation_handle_y) def draw_scene(self): self.pixmap = QPixmap(os.path.join(os.path.dirname(os.path.abspath(__file__)), "images", "Field.png")) painter = QPainter(self.pixmap) - pen = QPen(Qt.white) - pen.setWidth(2) - painter.setPen(pen) - - painter.setBrush(Qt.NoBrush) # Ensure no filling happens + grey_pen = QPen(QColor(127, 127, 127)) + grey_pen.setWidth(2) + painter.setPen(grey_pen) + for i, handle in enumerate(self.handles): + if i < len(self.coordinates) and i + 1 < len(self.coordinates): + robot1_pos = QPoint(self.coordinates[i][0], self.coordinates[i][1]) + robot2_pos = QPoint(self.coordinates[i+1][0], self.coordinates[i+1][1]) + painter.drawLine(handle, robot1_pos) + painter.drawLine(handle, robot2_pos) + if len(self.coordinates) >= 2: for i in range(len(self.coordinates) - 1): start = QPoint(self.coordinates[i][0], self.coordinates[i][1]) end = QPoint(self.coordinates[i + 1][0], self.coordinates[i + 1][1]) if i < len(self.handles): - # Draw cubic Bezier curve + pen = QPen(Qt.yellow) + pen.setWidth(2) + painter.setPen(pen) + path = QPainterPath() path.moveTo(start) handle = self.handles[i] - path.cubicTo( - handle, # Control point 1 - handle, # Control point 2 - end # End point - ) + path.cubicTo(handle, handle, end) painter.drawPath(path) - # Draw handle - painter.setBrush(QColor(255, 255, 255)) # White color for handle - painter.drawRect( + painter.setPen(Qt.NoPen) + painter.setBrush(QColor(0, 255, 255)) + painter.drawEllipse( handle.x() - self.handle_size // 2, handle.y() - self.handle_size // 2, self.handle_size, @@ -198,18 +231,33 @@ class MainWindow(QMainWindow): else: painter.drawLine(start, end) - # Create robots for i, (x, y) in enumerate(self.coordinates): - top_left_x = int(x - self.robot_size // 2) - top_left_y = int(y - self.robot_size // 2) - painter.drawRect(QRect(top_left_x, top_left_y, self.robot_size, self.robot_size)) - - # Draw robot number - painter.drawText(QRect(top_left_x, top_left_y, self.robot_size, self.robot_size), - Qt.AlignCenter, str(i + 1)) - + if i < len(self.rotation_handles): + angle = self.robot_angles[i] + + painter.save() + painter.translate(x, y) + painter.rotate(np.degrees(angle)) + + pen = QPen(QColor(127, 127, 127)) + pen.setWidth(2) + painter.setPen(pen) + painter.setBrush(Qt.NoBrush) + painter.drawRect(-self.robot_size // 2, -self.robot_size // 2, + self.robot_size, self.robot_size) + + painter.restore() + + painter.setPen(Qt.white) + font = painter.font() + font.setPointSize(25) + painter.setFont(font) + painter.drawText(QRect(x - self.robot_size//2, y - self.robot_size//2, + self.robot_size, self.robot_size), + Qt.AlignCenter, str(i + 1)) + for i, handle in enumerate(self.rotation_handles): - painter.setBrush(QColor(255, 0, 0)) # Red color for rotation handle + painter.setBrush(QColor(255, 0, 255)) painter.drawEllipse( handle, self.handle_size // 2, @@ -217,13 +265,6 @@ class MainWindow(QMainWindow): ) painter.setBrush(Qt.NoBrush) - # Draw line connecting robot and rotation handle - robot_x, robot_y = self.coordinates[i] - painter.drawLine( - QPoint(robot_x, robot_y), - handle - ) - self.image_label.setPixmap(self.pixmap) def clear_points(self):