|
|
@@ -0,0 +1,292 @@
|
|
|
+from collections import defaultdict
|
|
|
+from .McuDebug import McuDebug
|
|
|
+from functools import partial
|
|
|
+import time
|
|
|
+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)
|
|
|
+ self.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(value, 2)
|
|
|
+ if self.last_value[name] - 1 <= value <= self.last_value[name] + 1:
|
|
|
+ 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.doubleSpinBox.value())
|
|
|
+ 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.doubleSpinBox_2.value()
|
|
|
+ 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.doubleSpinBox_3.value()
|
|
|
+ device_name = "turntable_steering"
|
|
|
+ self.mcu.set_deviation(
|
|
|
+ device_name=device_name, _type=1, deviation=turntable_steering_deviation
|
|
|
+ )
|
|
|
+
|
|
|
+ if name == "翻板舵机中位":
|
|
|
+ # 设定翻版舵机偏移量
|
|
|
+ value = self.doubleSpinBox_4.value()
|
|
|
+ device_name = "overturn_steering"
|
|
|
+ self.mcu.set_deviation(
|
|
|
+ device_name=device_name, _type=0, deviation=value
|
|
|
+ )
|
|
|
+ if name == "翻板舵机高位":
|
|
|
+ # 设定翻版舵机偏移量
|
|
|
+ value = self.doubleSpinBox_5.value()
|
|
|
+ device_name = "overturn_steering"
|
|
|
+ self.mcu.set_deviation(
|
|
|
+ device_name=device_name, _type=1, deviation=value
|
|
|
+ )
|
|
|
+ if name == "翻板舵机上升速度":
|
|
|
+ # 设定翻版舵机偏移量
|
|
|
+ value = self.doubleSpinBox_6.value()
|
|
|
+ device_name = "overturn_steering"
|
|
|
+ self.mcu.set_deviation(
|
|
|
+ device_name=device_name, _type=2, deviation=value
|
|
|
+ )
|
|
|
+ if name == "翻板舵机下降速度":
|
|
|
+ # 设定翻版舵机偏移量
|
|
|
+ value = self.doubleSpinBox_8.value()
|
|
|
+ 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)
|
|
|
+
|
|
|
+ def get_mcu_deviation(self):
|
|
|
+ self.mcu.get_deviation()
|
|
|
+
|
|
|
+ def get_mcu_deviation_info(self, data):
|
|
|
+ if "_type" not in data:
|
|
|
+ return
|
|
|
+ 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"]))
|
|
|
+ # 相机舵机
|
|
|
+ if "camera_steering_deviation" in data:
|
|
|
+ self.doubleSpinBox_2.setValue(float(data["camera_steering_deviation"]))
|
|
|
+ # 转盘偏移位
|
|
|
+ if "turntable_steering_deviation" in data:
|
|
|
+ self.doubleSpinBox_3.setValue(
|
|
|
+ float(data["turntable_steering_deviation"])
|
|
|
+ )
|
|
|
+ # 翻板舵机中位
|
|
|
+ if "overturn_steering_middle" in data:
|
|
|
+ self.doubleSpinBox_4.setValue(float(data["overturn_steering_middle"]))
|
|
|
+ # 翻板舵机高位
|
|
|
+ if "overturn_steering_high" in data:
|
|
|
+ self.doubleSpinBox_5.setValue(float(data["overturn_steering_high"]))
|
|
|
+
|
|
|
+ # 翻板舵机
|
|
|
+ if "overturn_steering_up_speed" in data:
|
|
|
+ self.doubleSpinBox_6.setValue(float(data["overturn_steering_up_speed"]))
|
|
|
+
|
|
|
+ if "overturn_steering_down_speed" in data:
|
|
|
+ self.doubleSpinBox_8.setValue(
|
|
|
+ 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)
|
|
|
+
|
|
|
+ def __del__(self):
|
|
|
+ self.state = 2
|
|
|
+ self.t = None
|