CameraMachine/python/mcu/McuDeviationSet.py

from collections import defaultdict
from .McuDebug import McuDebug
from functools import partial
import time, asyncio
import settings


class McuDeviationSet:
    # 调试处理
    # data_sign = Signal(dict)
    # info_sign = Signal(str)
    # is_get_offset = Signal(dict)

    def __init__(self, mcu):
        super().__init__()
        self.mcu = mcu
        # self.setupUi(self)
        # self.init()
        # self.set_enable_by_mcu()
        # self.show()
        self.last_value = defaultdict(float)
        self.mcu_debug = McuDebug(mcu, is_debug=True, is_deviation=False)
        loop = asyncio.get_event_loop()
        loop.create_task(self.get_mcu_deviation(), name="get_mcu_deviation")
        # # 运动到设定位
        # QTimer.singleShot(2500, self.init_pos)

    # def set_enable_by_mcu(self):
    #     if self.mcu.state_camera_motor != 2:
    #         self.doubleSpinBox.setEnabled(False)
    #         self.pushButton_3.setEnabled(False)

    #     if self.mcu.state_camera_steering != 2:
    #         self.doubleSpinBox_2.setEnabled(False)
    #         self.pushButton_16.setEnabled(False)

    #     if self.mcu.state_turntable_steering != 2:
    #         self.doubleSpinBox_3.setEnabled(False)
    #         self.pushButton_22.setEnabled(False)
    #         self.doubleSpinBox_7.setEnabled(False)

    def init(self):
        self.is_get_offset.connect(self.init_pos)

        self.windows.data_info.connect(self.get_mcu_deviation_info)
        # 相机电机
        self.doubleSpinBox.setSingleStep(1)
        self.doubleSpinBox.setMinimum(0)
        self.doubleSpinBox.setMaximum(400)
        self.doubleSpinBox.valueChanged.connect(
            lambda x: self.change_value(x, "相机电机")
        )

        # 相机舵机
        self.doubleSpinBox_2.setSingleStep(0.1)
        self.doubleSpinBox_2.setMinimum(-40)
        self.doubleSpinBox_2.setMaximum(40)
        self.doubleSpinBox_2.valueChanged.connect(
            lambda x: self.change_value(x, "相机舵机")
        )

        # 转盘电机
        self.doubleSpinBox_3.setSingleStep(1)
        self.doubleSpinBox_3.setMinimum(-720)
        self.doubleSpinBox_3.setMaximum(720)
        self.doubleSpinBox_3.valueChanged.connect(
            lambda x: self.change_value(x, "转盘舵机")
        )

        # 转盘前后电机
        self.doubleSpinBox_7.setSingleStep(1)
        self.doubleSpinBox_7.setMinimum(0)
        self.doubleSpinBox_7.setMaximum(950)
        self.doubleSpinBox_7.setValue(300)
        self.doubleSpinBox_7.valueChanged.connect(
            lambda x: self.change_value(x, "转盘前后电机")
        )

        # 翻板舵机中位
        self.doubleSpinBox_4.setSingleStep(0.5)
        self.doubleSpinBox_4.setValue(90)
        self.doubleSpinBox_4.setMinimum(0)
        self.doubleSpinBox_4.setMaximum(180)
        self.doubleSpinBox_4.valueChanged.connect(
            lambda x: self.change_value(x, "翻板舵机中位")
        )

        # 翻板舵机高位
        self.doubleSpinBox_5.setSingleStep(0.5)
        self.doubleSpinBox_5.setValue(10)
        self.doubleSpinBox_5.setMinimum(0)
        self.doubleSpinBox_5.setMaximum(180)
        self.doubleSpinBox_5.valueChanged.connect(
            lambda x: self.change_value(x, "翻板舵机高位")
        )

        # 翻板舵机上升速度
        self.doubleSpinBox_6.setSingleStep(1)
        self.doubleSpinBox_6.setValue(5)
        self.doubleSpinBox_6.setMinimum(1)
        self.doubleSpinBox_6.setMaximum(10)
        self.doubleSpinBox_6.valueChanged.connect(
            lambda x: self.change_value(x, "翻板舵机上升速度")
        )

        # 翻板舵机下降速度
        self.doubleSpinBox_8.setSingleStep(1)
        self.doubleSpinBox_8.setValue(5)
        self.doubleSpinBox_8.setMinimum(1)
        self.doubleSpinBox_8.setMaximum(10)
        self.doubleSpinBox_8.valueChanged.connect(
            lambda x: self.change_value(x, "翻板舵机下降速度")
        )

        # 获取偏移量
        self.pushButton_2.clicked.connect(self.get_mcu_deviation)
        # 设定相机高度偏移量
        self.pushButton_3.clicked.connect(lambda *args: self.set_deviation("相机电机"))
        # 设定相机舵机偏移量
        self.pushButton_16.clicked.connect(lambda *args: self.set_deviation("相机舵机"))
        # 设定转盘偏移量
        self.pushButton_22.clicked.connect(lambda *args: self.set_deviation("转盘舵机"))
        # 设定翻版舵机偏移量
        self.pushButton_8.clicked.connect(
            lambda *args: self.set_deviation("翻板舵机中位")
        )
        # 设定翻版舵机偏移量
        self.pushButton_9.clicked.connect(
            lambda *args: self.set_deviation("翻板舵机高位")
        )
        self.pushButton_10.clicked.connect(
            lambda *args: self.set_deviation("翻板舵机上升速度")
        )
        self.pushButton_12.clicked.connect(
            lambda *args: self.set_deviation("翻板舵机下降速度")
        )

        # 转盘重新获取转速比
        # self.pushButton_7.clicked.connect(self.set_turntable_to_ratio_init)
        # 转盘重新定位到原点
        self.pushButton_11.clicked.connect(self._to_init_all)

    def init_pos(self, data):
        time.sleep(0.6)
        func = partial(self._move_equipment, data=data)
        self.do_thread_run(func, call_back=None, time_out=30)

    def _move_equipment(self, data):
        self.mcu_debug.move_equipment(data=data)
        # MCU运动是否有停止检查，设定超时时间
        time.sleep(1.5)
        if self.check_mcu_move_is_stop() is False:
            return

    def check_mcu_move_is_stop(self, out_time=15):
        _s = time.time()
        while 1:
            time.sleep(0.1)
            if time.time() - _s > out_time:
                return True
            if self.mcu.mcu_move_state == 2:
                return True
            if settings.IS_LIN_SHI_TEST:
                time.sleep(3)
                return True

    def change_value(self, value, name):
        value = round(float(value), 2)
        print("change_value===>", value, name)
        if name == "相机电机":
            self.mcu_debug.camera_high_motor(value=value)
        if name == "相机舵机":
            self.mcu_debug.camera_steering(value=value)
        if name == "转盘舵机":
            self.mcu_debug.turntable_steering(value=value)
        if name == "转盘前后电机":
            self.mcu_debug.turntable_position_motor(value=value)
        if name == "翻板舵机中位":
            self.mcu_debug.overturn_steering(value=value)
        if name == "翻板舵机高位":
            self.mcu_debug.overturn_steering(value=value)
        if name == "翻板舵机上升速度":
            pass
        print(value, name)
        self.last_value[name] = value

    def set_deviation(self, name):
        if name == "相机电机":
            # 设定相机高度偏移量 单位mm
            camera_high_motor_deviation = int(self.last_value[name])
            device_name = "camera_high_motor"
            self.mcu.set_deviation(
                device_name=device_name, _type=0, deviation=camera_high_motor_deviation
            )

        if name == "相机舵机":
            # 设定相机舵机偏移量
            camera_steering_deviation = self.last_value[name]
            device_name = "camera_steering"
            self.mcu.set_deviation(
                device_name=device_name, _type=0, deviation=camera_steering_deviation
            )
            # self.doubleSpinBox_2.setValue(0.0)

        if name == "转盘舵机":
            # 设定转盘舵机偏移角度  单位 度
            turntable_steering_deviation = self.last_value[name]
            device_name = "turntable_steering"
            self.mcu.set_deviation(
                device_name=device_name, _type=1, deviation=turntable_steering_deviation
            )

        if name == "翻板舵机中位":
            # 设定翻版舵机偏移量
            value = self.last_value[name]
            device_name = "overturn_steering"
            self.mcu.set_deviation(device_name=device_name, _type=0, deviation=value)
        if name == "翻板舵机高位":
            # 设定翻版舵机偏移量
            value = self.last_value[name]
            device_name = "overturn_steering"
            self.mcu.set_deviation(device_name=device_name, _type=1, deviation=value)
        if name == "翻板舵机上升速度":
            # 设定翻版舵机偏移量
            value = self.last_value[name]
            device_name = "overturn_steering"
            self.mcu.set_deviation(device_name=device_name, _type=2, deviation=value)
        if name == "翻板舵机下降速度":
            # 设定翻版舵机偏移量
            value = self.last_value[name]
            device_name = "overturn_steering"
            self.mcu.set_deviation(device_name=device_name, _type=3, deviation=value)

    def _to_init_all(self, *args):
        self.mcu.to_init_device_origin_point(device_name="mcu", is_force=True)

    async def get_mcu_deviation(self):
        await self.mcu.getDeviationInfo()

    def get_mcu_deviation_info(self, data):
        if "_type" not in data:
            return
        get_deviation_data = {}
        if data["_type"] == "get_deviation_data":
            data = data["data"]
            print("偏移量信息")
            # overturn_steering_up_speed  overturn_steering_down_speed
            # 相机电机 单位mm
            if "camera_high_motor_deviation" in data:
                # self.doubleSpinBox.setValue(float(data["camera_high_motor_deviation"]))
                get_deviation_data["camera_high_motor_deviation"] = float(
                    data["camera_high_motor_deviation"]
                )
            # 相机舵机
            if "camera_steering_deviation" in data:
                # self.doubleSpinBox_2.setValue(float(data["camera_steering_deviation"]))
                get_deviation_data["camera_steering_deviation"] = float(
                    data["camera_steering_deviation"]
                )
            # 转盘偏移位
            if "turntable_steering_deviation" in data:
                # self.doubleSpinBox_3.setValue(
                #     float(data["turntable_steering_deviation"])
                # )
                get_deviation_data["turntable_steering_deviation"] = float(
                    data["turntable_steering_deviation"]
                )
            # 翻板舵机中位
            if "overturn_steering_middle" in data:
                # self.doubleSpinBox_4.setValue(float(data["overturn_steering_middle"]))
                get_deviation_data["overturn_steering_middle"] = float(
                    data["overturn_steering_middle"]
                )
            # 翻板舵机高位
            if "overturn_steering_high" in data:
                # self.doubleSpinBox_5.setValue(float(data["overturn_steering_high"]))
                get_deviation_data["overturn_steering_high"] = float(
                    data["overturn_steering_high"]
                )

            # 翻板舵机
            if "overturn_steering_up_speed" in data:
                # self.doubleSpinBox_6.setValue(float(data["overturn_steering_up_speed"]))
                get_deviation_data["overturn_steering_up_speed"] = float(
                    data["overturn_steering_up_speed"]
                )

            if "overturn_steering_down_speed" in data:
                # self.doubleSpinBox_8.setValue(
                #     float(data["overturn_steering_down_speed"])
                # )
                get_deviation_data["overturn_steering_down_speed"] = float(
                    data["overturn_steering_down_speed"]
                )

            # 初始化位置
            # data = {
            #     "相机电机": self.doubleSpinBox.value(),
            #     "相机舵机": self.doubleSpinBox_2.value(),
            #     "转盘舵机": self.doubleSpinBox_3.value(),
            #     "转盘前后电机": self.doubleSpinBox_7.value(),
            # }
            # self.is_get_offset.emit(data)
            print("获取初始化位置  [get_deviation_data]  ", get_deviation_data)

    def __del__(self):
        self.state = 2
        self.t = None