# coding:utf-8 import sys sys.path.append("..") from pypibot import log import pypibot import params import dataholder from dataholder import MessageID from transport import Transport from PyQt5.QtWidgets import QApplication, QDialog from PyQt5.QtCore import QObject,pyqtSignal import pb import threading port = "COM4" pypibot.assistant.enableGlobalExcept() # log.enableFileLog(log_dir + "ros_$(Date8)_$(filenumber2).log") log.setLevel("i") class MainDialog(QDialog): encoder_signal = pyqtSignal(list) imu_signal = pyqtSignal(list) pid_debug_signal = pyqtSignal(list) def __init__(self, parent=None): super(QDialog, self).__init__(parent) self.ui = pb.Ui_pb() self.ui.setupUi(self) self.model_type_list = {"2wd-diff": dataholder.RobotModelType.MODEL_TYPE_2WD_DIFF, "4wd-diff": dataholder.RobotModelType.MODEL_TYPE_4WD_DIFF, "3wd-omni": dataholder.RobotModelType.MODEL_TYPE_3WD_OMNI, "4wd-omni": dataholder.RobotModelType.MODEL_TYPE_4WD_OMNI, "4wd-mecanum": dataholder.RobotModelType.MODEL_TYPE_4WD_MECANUM} self.model_value_list = {0: dataholder.RobotModelType.MODEL_TYPE_2WD_DIFF, 1: dataholder.RobotModelType.MODEL_TYPE_4WD_DIFF, 2: dataholder.RobotModelType.MODEL_TYPE_3WD_OMNI, 3: dataholder.RobotModelType.MODEL_TYPE_4WD_OMNI, 3: dataholder.RobotModelType.MODEL_TYPE_4WD_MECANUM} self.model_index_list = {dataholder.RobotModelType.MODEL_TYPE_2WD_DIFF: 0, dataholder.RobotModelType.MODEL_TYPE_4WD_DIFF: 1, dataholder.RobotModelType.MODEL_TYPE_3WD_OMNI: 2, dataholder.RobotModelType.MODEL_TYPE_4WD_OMNI: 3, dataholder.RobotModelType.MODEL_TYPE_4WD_MECANUM: 4} self.imu_type_list = {"gy65": 0, "gy85": 1, "gy87": 2} self.imu_value_list = {49: "gy65", 69: "gy85", 71: "gy87"} self.imu_index_list = {0: 49, 1: 69, 2: 71} self.init_ui() self.mboard = None self.init_dev() self.encoder_signal.connect(self.update_encoder) self.imu_signal.connect(self.update_imu) self.pid_debug_signal.connect(self.update_pid_debug) self._KeepRunning = True self.encoder_thread = threading.Thread(target=self._read_encoder) self.encoder_thread.start() def closeEvent(self, event): self._KeepRunning = False def init_ui(self): for model_type in self.model_type_list.keys(): self.ui.combox_model.addItem(model_type) for imu_type in self.imu_type_list.keys(): self.ui.combox_imu_type.addItem(imu_type) self.ui.slider_wheel_diameter.setMinimum(10) self.ui.slider_wheel_diameter.setMaximum(500) self.ui.slider_wheel_track.setMinimum(50) self.ui.slider_wheel_track.setMaximum(1000) self.ui.slider_encoder.setMinimum(1) self.ui.slider_encoder.setMaximum(500) self.ui.slider_motor_ratio.setMinimum(1) self.ui.slider_motor_ratio.setMaximum(1000) self.ui.slider_pid_interval.setMinimum(1) self.ui.slider_pid_interval.setMaximum(80) self.ui.slider_kp.setMinimum(0) self.ui.slider_kp.setMaximum(10000) self.ui.slider_ki.setMinimum(0) self.ui.slider_ki.setMaximum(32000) self.ui.slider_kd.setMinimum(0) self.ui.slider_kd.setMaximum(1000) self.ui.slider_ko.setMinimum(0) self.ui.slider_ko.setMaximum(1000) self.ui.slider_cmd_lasttime.setMinimum(0) self.ui.slider_cmd_lasttime.setMaximum(1000) self.ui.slider_vx_max.setMinimum(0) self.ui.slider_vx_max.setMaximum(500) self.ui.slider_vy_max.setMinimum(0) self.ui.slider_vy_max.setMaximum(500) self.ui.slider_va_max.setMinimum(0) self.ui.slider_va_max.setMaximum(2000) self.ui.comboBox_support_model.setVisible(False) self.ui.pushButton_load.setVisible(False) self.ui.pushButton_read.clicked.connect(self.read_params) self.ui.pushButton_set.clicked.connect(self.write_params) self.ui.pushButton_start.clicked.connect(self.start_motor) self.ui.pushButton_stop.clicked.connect(self.stop_motor) self.ui.pushButton_start_2.clicked.connect(self.start_control) self.ui.pushButton_stop_2.clicked.connect(self.stop_control) self.ui.slider_set_pwm1.setMinimum(-5000) self.ui.slider_set_pwm1.setMaximum(5000) self.ui.slider_set_pwm2.setMinimum(-5000) self.ui.slider_set_pwm2.setMaximum(5000) self.ui.slider_set_pwm3.setMinimum(-5000) self.ui.slider_set_pwm3.setMaximum(5000) self.ui.slider_set_pwm4.setMinimum(-5000) self.ui.slider_set_pwm4.setMaximum(5000) self.ui.tabWidget.setTabText(0, '1.参数配置') self.ui.tabWidget.setTabText(1, '2.电机测试') self.ui.tabWidget.setCurrentIndex(0) def update_param(self, param): log.i("type:%d %d" % (param.model_type, param.imu_type)) try: self.ui.combox_model.setCurrentIndex(self.model_index_list[param.model_type]) except Exception as e: print("model type err") try: self.ui.combox_imu_type.setCurrentIndex( self.imu_type_list[self.imu_value_list[param.imu_type]]) except Exception as e: print("imu type err") try: self.ui.slider_wheel_diameter.setSliderPosition( param.wheel_diameter) self.ui.slider_wheel_track.setSliderPosition(param.wheel_track) self.ui.slider_encoder.setSliderPosition(param.encoder_resolution) self.ui.slider_motor_ratio.setSliderPosition(param.motor_ratio) self.ui.checkBox_motor1.setChecked( param.motor_nonexchange_flag & 0x1) self.ui.checkBox_motor2.setChecked( param.motor_nonexchange_flag & 0x2) self.ui.checkBox_motor3.setChecked( param.motor_nonexchange_flag & 0x4) self.ui.checkBox_motor4.setChecked( param.motor_nonexchange_flag & 0x8) self.ui.checkBox_encoder1.setChecked( param.encoder_nonexchange_flag & 0x1) self.ui.checkBox_encoder2.setChecked( param.encoder_nonexchange_flag & 0x2) self.ui.checkBox_encoder3.setChecked( param.encoder_nonexchange_flag & 0x4) self.ui.checkBox_encoder4.setChecked( param.encoder_nonexchange_flag & 0x8) except Exception as e: print("motor dir param err") try: self.ui.slider_cmd_lasttime.setSliderPosition(param.cmd_last_time) self.ui.slider_vx_max.setSliderPosition(param.max_v_liner_x) self.ui.slider_vy_max.setSliderPosition(param.max_v_liner_y) self.ui.slider_va_max.setSliderPosition(param.max_v_angular_z) except Exception as e: print("pid param err") try: self.ui.slider_pid_interval.setSliderPosition( param.do_pid_interval) self.ui.slider_kp.setSliderPosition(param.kp) self.ui.slider_ki.setSliderPosition(param.ki) self.ui.slider_kd.setSliderPosition(param.kd) self.ui.slider_ko.setSliderPosition(param.ko) except Exception as e: print("velocity limit param err") def read_params(self): # get robot parameter robotParam = self.DataHolder[MessageID.ID_GET_ROBOT_PARAMETER] p = self.mboard.request(MessageID.ID_GET_ROBOT_PARAMETER) if p: log.info("model_type:%d wheel_diameter:%d wheel_track:%d encoder_resolution:%d" % (robotParam.param.model_type, robotParam.param.wheel_diameter, robotParam.param.wheel_track, robotParam.param.encoder_resolution )) log.info("do_pid_interval:%d kp:%d ki:%d kd:%d ko:%d" % (robotParam.param.do_pid_interval, robotParam.param.kp, robotParam.param.ki, robotParam.param.kd, robotParam.param.ko)) log.info("cmd_last_time:%d imu_type:%d" % (robotParam.param.cmd_last_time, robotParam.param.imu_type )) log.info("max_v:%d %d %d" % (robotParam.param.max_v_liner_x, robotParam.param.max_v_liner_y, robotParam.param.max_v_angular_z )) log.info("motor flag:%d encoder flag: %d" % (robotParam.param.motor_nonexchange_flag, robotParam.param.encoder_nonexchange_flag )) else: log.error('get params err') return False self.update_param(robotParam.param) def get_input_param(self): params = dataholder.RobotParameters() params.wheel_diameter = self.ui.slider_wheel_diameter.sliderPosition() params.wheel_track = self.ui.slider_wheel_track.sliderPosition() params.encoder_resolution = self.ui.slider_encoder.sliderPosition() params.do_pid_interval = self.ui.slider_pid_interval.sliderPosition() params.kp = self.ui.slider_kp.sliderPosition() params.ki = self.ui.slider_ki.sliderPosition() params.kd = self.ui.slider_kd.sliderPosition() params.ko = self.ui.slider_ko.sliderPosition() params.cmd_last_time = self.ui.slider_cmd_lasttime.sliderPosition() params.max_v_liner_x = self.ui.slider_vx_max.sliderPosition() params.max_v_liner_y = self.ui.slider_vy_max.sliderPosition() params.max_v_angular_z = self.ui.slider_va_max.sliderPosition() params.motor_ratio = self.ui.slider_motor_ratio.sliderPosition() params.imu_type = self.imu_index_list[self.ui.combox_imu_type.currentIndex( )] params.model_type = self.model_value_list[self.ui.combox_model.currentIndex()] params.motor_nonexchange_flag = 0 if self.ui.checkBox_motor1.isChecked(): params.motor_nonexchange_flag = params.motor_nonexchange_flag | 0x1 else: params.motor_nonexchange_flag = params.motor_nonexchange_flag & 0xfe if self.ui.checkBox_motor2.isChecked(): params.motor_nonexchange_flag = params.motor_nonexchange_flag | 0x2 else: params.motor_nonexchange_flag = params.motor_nonexchange_flag & 0xfd if self.ui.checkBox_motor3.isChecked(): params.motor_nonexchange_flag = params.motor_nonexchange_flag | 0x4 else: params.motor_nonexchange_flag = params.motor_nonexchange_flag & 0xfb if self.ui.checkBox_motor4.isChecked(): params.motor_nonexchange_flag = params.motor_nonexchange_flag | 0x8 else: params.motor_nonexchange_flag = params.motor_nonexchange_flag & 0xf7 params.encoder_nonexchange_flag = 0 if self.ui.checkBox_encoder1.isChecked(): params.encoder_nonexchange_flag = params.encoder_nonexchange_flag | 0x1 else: params.encoder_nonexchange_flag = params.encoder_nonexchange_flag & 0xfe if self.ui.checkBox_encoder2.isChecked(): params.encoder_nonexchange_flag = params.encoder_nonexchange_flag | 0x2 else: params.encoder_nonexchange_flag = params.encoder_nonexchange_flag & 0xfd if self.ui.checkBox_encoder3.isChecked(): params.encoder_nonexchange_flag = params.encoder_nonexchange_flag | 0x4 else: params.encoder_nonexchange_flag = params.encoder_nonexchange_flag & 0xfb if self.ui.checkBox_encoder4.isChecked(): params.encoder_nonexchange_flag = params.encoder_nonexchange_flag | 0x8 else: params.encoder_nonexchange_flag = params.encoder_nonexchange_flag & 0xf7 return params def write_params(self): self.DataHolder[MessageID.ID_SET_ROBOT_PARAMETER].param = self.get_input_param() p = self.mboard.request(MessageID.ID_SET_ROBOT_PARAMETER) if p: log.info('set parameter success') else: log.error('set parameter err') quit(1) def update_pid_debug(self, pid_data): self.ui.label_input_1.setText(str(pid_data[0])) self.ui.label_input_2.setText(str(pid_data[1])) self.ui.label_input_3.setText(str(pid_data[2])) self.ui.label_input_4.setText(str(pid_data[3])) self.ui.label_output_1.setText(str(pid_data[4])) self.ui.label_output_2.setText(str(pid_data[5])) self.ui.label_output_3.setText(str(pid_data[6])) self.ui.label_output_4.setText(str(pid_data[7])) def update_imu(self, imu): #log.info('imu: %s'%(('\t\t').join([str(x) for x in imu]))) self.ui.label_acc_x.setText(str(round(imu[0], 6))) self.ui.label_acc_y.setText(str(round(imu[1], 6))) self.ui.label_acc_z.setText(str(round(imu[2], 6))) self.ui.label_gyro_x.setText(str(round(imu[3], 6))) self.ui.label_gyro_y.setText(str(round(imu[4], 6))) self.ui.label_gyro_z.setText(str(round(imu[5], 6))) self.ui.label_magn_x.setText(str(round(imu[6], 6))) self.ui.label_magn_y.setText(str(round(imu[7], 6))) self.ui.label_magn_z.setText(str(round(imu[8], 6))) def update_encoder(self, encoder): log.debug('encoder count: %s'%(('\t\t').join([str(x) for x in encoder]))) self.ui.label_feedback1.setText(str(encoder[0])) self.ui.label_feedback2.setText(str(encoder[1])) self.ui.label_feedback3.setText(str(encoder[2])) self.ui.label_feedback4.setText(str(encoder[3])) def _read_encoder(self): while self._KeepRunning: robot_encoder = self.DataHolder[MessageID.ID_GET_ENCODER_COUNT].encoder p = self.mboard.request(MessageID.ID_GET_ENCODER_COUNT) if p: # log.info('encoder count: %s'%(('\t\t').join([str(x) for x in robot_encoder]))) self.encoder_signal.emit([int(x) for x in robot_encoder]) robot_imu = self.DataHolder[MessageID.ID_GET_IMU].imu p = self.mboard.request(MessageID.ID_GET_IMU) if p: # log.info('imu: %s'%(('\t\t').join([str(x) for x in robot_imu]))) self.imu_signal.emit([x for x in robot_imu]) pid_data = self.DataHolder[MessageID.ID_GET_PID_DEBUG].pid_data p = self.mboard.request(MessageID.ID_GET_PID_DEBUG) if p: # log.info('imu: %s'%(('\t\t').join([str(x) for x in robot_imu]))) self.pid_debug_signal.emit([x for x in pid_data]) import time time.sleep(0.1) def start_motor(self): self.DataHolder[MessageID.ID_SET_MOTOR_PWM].pwm = [self.ui.slider_set_pwm1.sliderPosition(), self.ui.slider_set_pwm2.sliderPosition(), self.ui.slider_set_pwm3.sliderPosition(), self.ui.slider_set_pwm4.sliderPosition()] p = self.mboard.request(MessageID.ID_SET_MOTOR_PWM) if p: log.info('set pwm success') else: log.error('set pwm err') def stop_motor(self): self.DataHolder[MessageID.ID_SET_MOTOR_PWM].pwm = [0]*4 p = self.mboard.request(MessageID.ID_SET_MOTOR_PWM) if p: log.info('set pwm success') else: log.error('set pwm err') def start_control(self): self.DataHolder[MessageID.ID_SET_VEL].v_liner_x = 200 self.DataHolder[MessageID.ID_SET_VEL].v_liner_y = 0 self.DataHolder[MessageID.ID_SET_VEL].v_angular_z = 10 p = self.mboard.request(MessageID.ID_SET_VEL) if p: log.info('set vel success') else: log.error('set vel err') def stop_control(self): self.DataHolder[MessageID.ID_SET_VEL].v_liner_x = 0 self.DataHolder[MessageID.ID_SET_VEL].v_liner_y = 0 self.DataHolder[MessageID.ID_SET_VEL].v_angular_z = 0 p = self.mboard.request(MessageID.ID_SET_VEL) if p: log.info('set vel success') else: log.error('set vel err') def init_dev(self): self.mboard = Transport(port, params.pibotBaud) if not self.mboard.start(): log.error("can not open %s" % port) return False self.DataHolder = self.mboard.getDataHolder() for num in range(0, 3): log.info("****************get robot version*****************") boardVersion = self.DataHolder[MessageID.ID_GET_VERSION] p = self.mboard.request(MessageID.ID_GET_VERSION) if p: log.info("firmware version:%s buildtime:%s" % ( boardVersion.version.decode(), boardVersion.build_time.decode())) break else: log.error('read firmware version err') import time time.sleep(1) if num == 2: log.error('please check connection or baudrate') return False return self.read_params() if __name__ == '__main__': app = QApplication(sys.argv) import qdarkstyle app.setStyleSheet(qdarkstyle.load_stylesheet(qt_api='pyqt5')) # from qt_material import apply_stylesheet # apply_stylesheet(app, theme='light_teal.xml') myDlg = MainDialog() myDlg.show() sys.exit(app.exec_())