/
AndreyNovikov
/
Champ
Обзор
Документация
Войти
/
AndreyNovikov
/
Champ
Код
Запросы
0
Задачи
Вики
Пакеты
0
Релизы
0
CI/CD
Аналитика
Безопасность
Module_A
Module_C.py
578 строк
22 KB
rizz
ЧModule_A_Update
12 авг 2025, 13:09
12 авг 2025, 13:09
014a979
Код
Авторство
О чём код?
from PyQt5 import QtCore, QtWidgets, QtGui from PyQt5.QtCore import Qt from motion.core import RobotControl, LedLamp, InterpreterStates, Waypoint import sys, math, datetime, time, csv, GUI, cv2 import numpy as np class MainWindow(QtWidgets.QMainWindow, GUI.Ui_MainWindow): def __init__(self): super().__init__() self.setupUi(self) self.robot = RobotControl('192.168.2.100') self.lamp = LedLamp('192.168.2.101') # self.cap = cv2.VideoCapture('Команда_1_5_1.mp4') self.BtVideo.clicked.connect(self.Video) self.BtCamera.clicked.connect(self.Camera) self.videoActive = True self.cameraActive = True self.selected_color = 'Синий' self.selected_form = 'Квадрат' self.color_Active = False self.form_Active = False self.BtDetectForm.clicked.connect(self.toggle_form) self.BtDetectColor.clicked.connect(self.toggle_color) self.timer_cam = QtCore.QTimer() self.timer_cam.timeout.connect(self.update_frame) self.timer_cam.start(30) # Список точек self.points = [] # Флаги self.gripper = 0 self.onOff = False self.connect = False self.pause = False self.cartActive = False self.jointActive = False # Buttons self.BtOnOff.clicked.connect(self.OnOff) self.BtPause.clicked.connect(self.Pause) self.BtStop.clicked.connect(self.Stop) self.BtManualCart.clicked.connect(self.ManualCart) self.BtManualJoint.clicked.connect(self.ManualJoint) self.BtGripper.clicked.connect(self.Gripper) self.BtToStart.clicked.connect(self.ToStart) self.BtPath.clicked.connect(self.ChoosePath) self.BtSaveToFile.clicked.connect(self.SaveToFile) self.BtAddPoint.clicked.connect(self.AddPoint) self.BtPlay.clicked.connect(self.PlayPoints) self.BtClearList.clicked.connect(self.ClearList) self.BtVideo.clicked.connect(self.Video) self.BtCamera.clicked.connect(self.Camera) # Logs self.logs = [] self.log_model = QtCore.QStringListModel() self.LVLogs.setModel(self.log_model) # Motors self.motors = QtCore.QTimer() self.motors.timeout.connect(self.update_motors) self.motors.start(500) # Pose self.pose = QtCore.QTimer() self.pose.timeout.connect(self.update_pose) self.pose.start(500) # Status indicators (lamp + labels) self.current_status_code = None self.status_timer = QtCore.QTimer() self.status_timer.timeout.connect(self.update_status) self.status_timer.start(1000) def add_log(self, message: str) -> None: now = datetime.datetime.now().strftime("%H:%M:%S") msg = f"{now} - {message}" self.logs.append(msg) self.log_model.setStringList(self.logs) def SaveToFile(self): filename = self.LECV.text().strip() if not filename: QtWidgets.QMessageBox.warning(self, "Ошибка", "Введите название файла") return if not filename.endswith('.txt'): filename += '.txt' try: with open(filename, 'a', encoding='utf-8') as f: f.write('\n'.join(self.logs) + '\n') self.add_log(f'Запись в файл {filename}') except Exception as e: QtWidgets.QMessageBox.warning(self, "Ошибка", f"Не удалось записать файл: {e}") def ChoosePath(self) -> None: start_dir = self.LECV.text().strip() or QtCore.QDir.homePath() filename, _ = QtWidgets.QFileDialog.getSaveFileName( self, "Выберите файл для логов", start_dir, "Text Files (*.txt);;All Files (*)" ) if not filename: return if not filename.endswith('.txt'): filename += '.txt' self.LECV.setText(filename) self.add_log(f'Выбран файл для логов: {filename}') def set_indicators(self, code: str) -> None: if code == self.current_status_code: return self.current_status_code = code try: self.lamp.setLamp(code) except Exception: pass # Reset all labels self.LStopped.setStyleSheet('') self.LPause.setStyleSheet('') self.LWork.setStyleSheet('') self.LWait.setStyleSheet('') # Apply mapping if code == '1000': # Wait self.LWait.setStyleSheet('background-color: blue') elif code == '0100': # Work self.LWork.setStyleSheet('background-color: green') elif code == '0010': # Pause self.LPause.setStyleSheet('background-color: yellow') elif code == '0001': # Stop self.LStopped.setStyleSheet('background-color: red') else: # '0000' or any other -> clear all pass def compute_status_code(self) -> str: if not self.onOff: return '0000' try: state = self.robot.getActualStateOut() if state == InterpreterStates.PROGRAM_STOP_S.value: return '0001' if state == InterpreterStates.PROGRAM_PAUSE_S.value: return '0010' if state == InterpreterStates.PROGRAM_IS_DONE.value: return '1000' # Default active work state return '0100' except Exception: # If cannot read state, assume safe wait when motors are on return '1000' def update_status(self) -> None: code = self.compute_status_code() self.set_indicators(code) def Camera(self): if self.cameraActive == False: self.cap = cv2.VideoCapture(3) self.BtVideo.setEnabled(False) self.cameraActive = True self.add_log('Возпроизведено изображение с камеры') else: self.cap = cv2.VideoCapture() self.BtVideo.setEnabled(True) self.cameraActive = False def Video(self): if self.videoActive == False: self.cap = cv2.VideoCapture('Команда_1_5_1.mp4') self.BtCamera.setEnabled(False) self.videoActive = True self.add_log('Возпроизведено видео') else: self.cap = cv2.VideoCapture() self.BtCamera.setEnabled(True) self.videoActive = False def update_frame(self): ret, frame = self.cap.read() if not ret: return self.show_image(self.LCamera1, cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)) if self.color_Active: mask = self.get_color_mask(frame) else: mask = np.ones(frame.shape[:2], np.uint8)*255 mask = cv2.morphologyEx(mask, cv2.MORPH_CLOSE, np.ones((7, 7), np.uint8)) # contours, _ = cv2.findContours([mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE]) contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)[-2:] for c in contours: area = cv2.contourArea(c) if area < 300: continue approx = cv2.approxPolyDP(c, 0.04 * cv2.arcLength(c, True), True) form = self.get_shape(c, approx) if not self.form_Active or form == self.selected_form: cv2.drawContours(frame, [approx], -1, (0, 255, 0), 2) M = cv2.moments(c) cx, cy = (int(M['m10']/M['m00']), int(M['m01']/M['m00'])) if M['m00'] else [0,0] color = self.selected_color if self.color_Active else '-' print(f'Объект: {object}, Цвет: {color}, Центр: {cx}, {cy}, Площадь:{int(area)}') self.add_log(f'Объект: {object}, Цвет: {color}, Центр: {cx}, {cy}, Площадь:{int(area)}') self.show_image(self.LCamera2, cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)) def get_color_mask(self, frame): # color_ranges = { # 'Синий': ([100, 100, 50], [130, 255, 255]), # 'Красный': ([0, 100, 100], [10, 255, 255]), # 'Зеленый': ([40, 70, 70], [80, 255, 255]) # } color_ranges = { 'Синий': ([100, 100, 90], [130, 255, 255]), 'Красный': ([0, 100, 135], [10, 255, 255]), 'Зеленый': ([40, 70, 50], [100, 255, 200]) } hsv = cv2.cvtColor(frame,cv2.COLOR_BGR2HSV) lower, upper = map(np.array, color_ranges[self.selected_color]) return cv2.inRange(hsv, lower, upper) def get_shape(self, c, approx): if len(approx) == 3: return "Треугольник" w, h = cv2.minAreaRect(c)[1] area = cv2.contourArea(c) rect_area = w*h if w and h and 0.6 < min(w, h)/max(w, h)<1.4 and area/ rect_area > 0.5: return 'Квадрат' (_, _), radius = cv2.minEnclosingCircle(c) if area/ (np.pi*radius**2) > 0.5: return 'Круг' return 'Квадрат' def show_image(self, label, img): h, w, ch = img.shape gimg = QtGui.QImage(img.data, w, h, ch*w, QtGui.QImage.Format_RGB888) label.setPixmap(QtGui.QPixmap.fromImage(gimg).scaled(label.width(), label.height(), QtCore.Qt.KeepAspectRatio)) def closeEvent(self, e): if hasattr(self, 'cap'): self.cap.release() e.accept() def toggle_color(self): self.color_Active = not self.color_Active if self.color_Active: self.selected_color = self.CBColor.currentText() self.BtDetectColor.setText('Сброс детекции') else: self.selected_color = None self.BtDetectColor.setText('Detect color') def toggle_form(self): self.form_Active = not self.form_Active if self.form_Active: self.selected_form = self.CBForm.currentText() self.BtDetectForm.setText('Сброс детекции') else: self.selected_form = None self.BtDetectForm.setText('Detect form') def connect_cart_sliders(self): self.Slider1.valueChanged.connect(self.update_cart_velocity) self.Slider2.valueChanged.connect(self.update_cart_velocity) self.Slider3.valueChanged.connect(self.update_cart_velocity) self.Slider4.valueChanged.connect(self.update_cart_velocity) self.Slider5.valueChanged.connect(self.update_cart_velocity) self.Slider6.valueChanged.connect(self.update_cart_velocity) def disconnect_cart_sliders(self): self.Slider1.valueChanged.disconnect(self.update_cart_velocity) self.Slider2.valueChanged.disconnect(self.update_cart_velocity) self.Slider3.valueChanged.disconnect(self.update_cart_velocity) self.Slider4.valueChanged.disconnect(self.update_cart_velocity) self.Slider5.valueChanged.disconnect(self.update_cart_velocity) self.Slider6.valueChanged.disconnect(self.update_cart_velocity) def connect_joint_sliders(self): self.Slider1.valueChanged.connect(self.update_joint_velocity) self.Slider2.valueChanged.connect(self.update_joint_velocity) self.Slider3.valueChanged.connect(self.update_joint_velocity) self.Slider4.valueChanged.connect(self.update_joint_velocity) self.Slider5.valueChanged.connect(self.update_joint_velocity) self.Slider6.valueChanged.connect(self.update_joint_velocity) def disconnect_joint_sliders(self): self.Slider1.valueChanged.disconnect(self.update_joint_velocity) self.Slider2.valueChanged.disconnect(self.update_joint_velocity) self.Slider3.valueChanged.disconnect(self.update_joint_velocity) self.Slider4.valueChanged.disconnect(self.update_joint_velocity) self.Slider5.valueChanged.disconnect(self.update_joint_velocity) self.Slider6.valueChanged.disconnect(self.update_joint_velocity) def update_cart_velocity(self): vx = self.Slider1.value() / 100 vy = self.Slider2.value() / 100 vz = self.Slider3.value() / 100 velocity = [vx, vz, vy, 0, 0, 0] self.robot.setCartesianVelocity(velocity) def update_joint_velocity(self): v1 = self.Slider1.value() / 100 v2 = self.Slider2.value() / 100 v3 = self.Slider3.value() / 100 v4 = self.Slider1.value() / 100 v5 = self.Slider2.value() / 100 v6 = self.Slider3.value() / 100 velocity = [v1, v2, v3, v4, v5, v6] self.robot.setJointVelocity(velocity) def update_motors(self): tick = self.robot.getMotorPositionTick() radian = self.robot.getMotorPositionRadians() degrees = [math.degrees(r) for r in radian] for i in range(6): self.TableInfo.setItem(0, i, QtWidgets.QTableWidgetItem(str(round(tick[i], 3)))) for i in range(6): self.TableInfo.setItem(1, i, QtWidgets.QTableWidgetItem(str(round(radian[i], 3)))) for i in range(6): self.TableInfo.setItem(2, i, QtWidgets.QTableWidgetItem(str(round(degrees[i], 3)))) def update_pose(self): tool_pose = self.robot.getToolPosition() self.TablePose.setItem(0, 0, QtWidgets.QTableWidgetItem(str(round(tool_pose[0], 3)))) self.TablePose.setItem(0, 1, QtWidgets.QTableWidgetItem(str(round(tool_pose[1], 3)))) self.TablePose.setItem(0, 2, QtWidgets.QTableWidgetItem(str(round(tool_pose[2], 3)))) self.TablePose.setItem(0, 3, QtWidgets.QTableWidgetItem(str(self.gripper))) def OnOff(self): self.robot.connect() self.BtOnOff.setText("Off") self.lamp.setLamp("1111") self.LStopped.setStyleSheet('background-color: blue') self.LPause.setStyleSheet('background-color: blue') self.LWork.setStyleSheet('background-color: blue') self.LWait.setStyleSheet('background-color: blue') self.add_log('Робот включен') if not self.onOff: self.robot.engage() self.onOff = True self.add_log('Моторы запущены') self.lamp.setLamp('1000') self.LStopped.setStyleSheet('') self.LPause.setStyleSheet('') self.LWork.setStyleSheet('') self.LWait.setStyleSheet('background-color: blue') else: self.onOff = False self.robot.disengage() self.add_log('Моторы выключены') self.lamp.setLamp('0000') self.LStopped.setStyleSheet('') self.LPause.setStyleSheet('') self.LWork.setStyleSheet('') self.LWait.setStyleSheet('') def Pause(self): if not self.pause: self.pause = False self.robot.pause() self.add_log('Робот на паузе') self.lamp.setLamp('0010') self.LStopped.setStyleSheet('') self.LPause.setStyleSheet('background-color: blue') self.LWork.setStyleSheet('') self.LWait.setStyleSheet('') else: self.pause = True self.lamp.setLamp('1000') self.LStopped.setStyleSheet('') self.LPause.setStyleSheet('') self.LWork.setStyleSheet('') self.LWait.setStyleSheet('background-color: blue') if self.robot.moveToStart(): self.robot.play self.add_log('Робот на паузе') else: QtWidgets.QMessageBox.warning(self, "Ошибка", "Не удалось переместиться в стартовую позицию") def Stop(self): if self.robot.stop(): self.add_log('Экстренное торможение') self.lamp.setLamp('0001') self.LStopped.setStyleSheet('background-color: blue') self.LPause.setStyleSheet('') self.LWork.setStyleSheet('') self.LWait.setStyleSheet('') else: QtWidgets.QMessageBox.warning(self, "Ошибка", "Не удалось совершить экстренное торможение") def ToStart(self): self.robot.moveToStart() self.add_log('Перемещение в стартовую позицию') self.lamp.setLamp('1000') self.LStopped.setStyleSheet('') self.LPause.setStyleSheet('') self.LWork.setStyleSheet('background-color: blue') self.LWait.setStyleSheet('') time.sleep(1.0) self.LStopped.setStyleSheet('') self.LPause.setStyleSheet('') self.LWork.setStyleSheet('') self.LWait.setStyleSheet('background-color: blue') def Gripper(self): if not self.gripper: self.robot.toolON() self.gripper = 1 self.BtGripper.setText("Off") self.add_log('Захват включен') time.sleep(1.0) self.lamp.setLamp('0100') self.LStopped.setStyleSheet('') self.LPause.setStyleSheet('') self.LWork.setStyleSheet('background-color: blue') self.LWait.setStyleSheet('') else: self.robot.toolOFF() self.gripper = 0 self.BtGripper.setText("On") self.add_log('Захват выключен') time.sleep(1.0) self.lamp.setLamp('1000') self.LStopped.setStyleSheet('') self.LPause.setStyleSheet('') self.LWork.setStyleSheet('') self.LWait.setStyleSheet('background-color: blue') def ManualCart(self): if not self.cartActive: self.connect_cart_sliders() self.cartActive = True self.BtManualJoint.setEnabled(False) self.robot.manualCartMode() self.update_cart_velocity() self.add_log('Активирован режим управления в декартовых координатах') self.lamp.setLamp('0100') self.LStopped.setStyleSheet('') self.LPause.setStyleSheet('') self.LWork.setStyleSheet('background-color: blue') self.LWait.setStyleSheet('') else: self.disconnect_cart_sliders() self.cartActive = False self.BtManualJoint.setEnabled(True) self.add_log('Режим управления в декартовых координатах выключен') self.lamp.setLamp('1000') self.LStopped.setStyleSheet('') self.LPause.setStyleSheet('') self.LWork.setStyleSheet('') self.LWait.setStyleSheet('background-color: blue') def ManualJoint(self): if not self.cartActive: self.connect_cart_sliders() self.cartActive = True self.BtManualCart.setEnabled(False) self.robot.manualCartMode() self.update_cart_velocity() self.add_log('Активирован ручной режим управления') self.lamp.setLamp('0100') self.LStopped.setStyleSheet('') self.LPause.setStyleSheet('') self.LWork.setStyleSheet('background-color: blue') self.LWait.setStyleSheet('') else: self.disconnect_joint_sliders() self.jointActive = False self.BtManualCart.setEnabled(True) self.add_log('Ручной режим управления выключен') self.lamp.setLamp('1000') self.LStopped.setStyleSheet('') self.LPause.setStyleSheet('') self.LWork.setStyleSheet('') self.LWait.setStyleSheet('background-color: blue') def AddPoint(self): x = float(self.TableCoordinats.item(0, 0).text()) y = float(self.TableCoordinats.item(0, 1).text()) z = float(self.TableCoordinats.item(0, 2).text()) gripper = int(self.TableCoordinats.item(0, 3).text()) point = [x, y, z, gripper] self.points.append(point) self.add_log('Точка добавлена в список') self.lamp.setLamp('1000') def PlayPoints(self): if not self.points: QtWidgets.QMessageBox.warning(self, "Ошибка", "Список точек пуст") cycles = self.SBCycle.value() if cycles < 1: self.SBCycle.setValue(1) cycles = 1 waypoints = [] gripper_states = [] for point in self.points: pose = [float(point[0]), float(point[1]), float(point[2]), 0, 0, 0] gripper = int(point[3]) waypoints.append(pose) gripper_states.append(gripper) for _ in range(cycles): for wp, gripper in zip(waypoints, gripper_states): self.robot.moveToPointL([Waypoint([wp])], rotational_acceleration=1.0, rotational_velocity=2.0) self.robot.play() self.lamp.setLamp('0100') time.sleep(1.0) if gripper == 1: self.robot.toolON() time.sleep(1.0) else: self.robot.toolOFF() time.sleep(1.0) self.add_log(f'Перемещение в точку: {wp}, Gripper: {gripper}') self.lamp.setLamp('1000') def ClearList(self): self.points.clear() self.add_log('Список точек очищен') self.lamp.setLamp('1000') def ToFile(self): filename = self.LEFile.text().strip() if not filename: QtWidgets.QMessageBox.warning(self, "Ошибка", "Введите название файла") if filename.endswith('.csv'): filename += '.csv' with open(filename, 'w', newline='') as f: csv.writer(f).writerows(self.points) self.add_log(f'Список точек сохранены в файл: {filename}') self.lamp.setLamp('1000') def main(): app = QtWidgets.QApplication(sys.argv) window = MainWindow() window.show() sys.exit(app.exec_()) if __name__ == "__main__": main()