diff --git a/Hardware/UPbot-Tools/__pycache__/dataholder.cpython-37.pyc b/Hardware/UPbot-Tools/__pycache__/dataholder.cpython-37.pyc new file mode 100644 index 0000000..56c3d17 Binary files /dev/null and b/Hardware/UPbot-Tools/__pycache__/dataholder.cpython-37.pyc differ diff --git a/Hardware/UPbot-Tools/__pycache__/params.cpython-37.pyc b/Hardware/UPbot-Tools/__pycache__/params.cpython-37.pyc new file mode 100644 index 0000000..b7bd56f Binary files /dev/null and b/Hardware/UPbot-Tools/__pycache__/params.cpython-37.pyc differ diff --git a/Hardware/UPbot-Tools/__pycache__/pb.cpython-37.pyc b/Hardware/UPbot-Tools/__pycache__/pb.cpython-37.pyc new file mode 100644 index 0000000..dce2291 Binary files /dev/null and b/Hardware/UPbot-Tools/__pycache__/pb.cpython-37.pyc differ diff --git a/Hardware/UPbot-Tools/__pycache__/transport.cpython-37.pyc b/Hardware/UPbot-Tools/__pycache__/transport.cpython-37.pyc new file mode 100644 index 0000000..e4ea487 Binary files /dev/null and b/Hardware/UPbot-Tools/__pycache__/transport.cpython-37.pyc differ diff --git a/Hardware/UPbot-Tools/app.py b/Hardware/UPbot-Tools/app.py new file mode 100644 index 0000000..2e0aaac --- /dev/null +++ b/Hardware/UPbot-Tools/app.py @@ -0,0 +1,434 @@ +# 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 = "COM7" + +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_()) diff --git a/Hardware/UPbot-Tools/dataholder.py b/Hardware/UPbot-Tools/dataholder.py new file mode 100644 index 0000000..a9e80c5 --- /dev/null +++ b/Hardware/UPbot-Tools/dataholder.py @@ -0,0 +1,250 @@ +import struct + +params_size=29 + +# main board +class MessageID: + ID_GET_VERSION = 0 + ID_SET_ROBOT_PARAMETER = 1 + ID_GET_ROBOT_PARAMETER = 2 + ID_INIT_ODOM = 3 + ID_SET_VEL = 4 + ID_GET_ODOM = 5 + ID_GET_PID_DEBUG = 6 + ID_GET_IMU = 7 + ID_GET_ENCODER_COUNT = 8 + ID_SET_MOTOR_PWM = 9 + +class RobotMessage: + def pack(self): + return b'' + + def unpack(self): + return True + +class RobotFirmwareInfo(RobotMessage): + def __init__(self): + self.version = '' + self.build_time = '' + + def unpack(self, data): + try: + upk = struct.unpack('16s16s', bytes(data)) + except: + return False + [self.version, self.build_time] = upk + return True + +class RobotImuType: + IMU_TYPE_GY65 = 49 + IMU_TYPE_GY85 = 69 + IMU_TYPE_GY87 = 71 + +class RobotModelType: + MODEL_TYPE_2WD_DIFF = 1 + MODEL_TYPE_4WD_DIFF = 2 + MODEL_TYPE_3WD_OMNI = 101 + MODEL_TYPE_4WD_OMNI = 102 + MODEL_TYPE_4WD_MECANUM = 201 + +class RobotParameters(): + def __init__(self, wheel_diameter=0, \ + wheel_track=0, \ + encoder_resolution=0, \ + do_pid_interval=0, \ + kp=0, \ + ki=0, \ + kd=0, \ + ko=0, \ + cmd_last_time=0, \ + max_v_liner_x=0, \ + max_v_liner_y=0, \ + max_v_angular_z=0, \ + imu_type=0, \ + motor_ratio=0, \ + model_type=0, \ + motor_nonexchange_flag=255, \ + encoder_nonexchange_flag=255, \ + ): + self.wheel_diameter = wheel_diameter + self.wheel_track = wheel_track + self.encoder_resolution = encoder_resolution + self.do_pid_interval = do_pid_interval + self.kp = kp + self.ki = ki + self.kd = kd + self.ko = ko + self.cmd_last_time = cmd_last_time + self.max_v_liner_x = max_v_liner_x + self.max_v_liner_y = max_v_liner_y + self.max_v_angular_z = max_v_angular_z + self.imu_type = imu_type + self.motor_ratio = motor_ratio + self.model_type = model_type + self.motor_nonexchange_flag = motor_nonexchange_flag + self.encoder_nonexchange_flag = encoder_nonexchange_flag + reserve=b'\xff' + self.reserve=b'' + for i in range(64-params_size): + self.reserve+=reserve +robotParam = RobotParameters() + +class GetRobotParameters(RobotMessage): + def __init__(self): + self.param = robotParam + + def unpack(self, data): + #print(bytes(data), len(bytes(data))) + upk = struct.unpack('<3H1B8H1B1H3B%ds'%(64-params_size), bytes(data)) + + [self.param.wheel_diameter, + self.param.wheel_track, + self.param.encoder_resolution, + self.param.do_pid_interval, + self.param.kp, + self.param.ki, + self.param.kd, + self.param.ko, + self.param.cmd_last_time, + self.param.max_v_liner_x, + self.param.max_v_liner_y, + self.param.max_v_angular_z, + self.param.imu_type, + self.param.motor_ratio, + self.param.model_type, + self.param.motor_nonexchange_flag, + self.param.encoder_nonexchange_flag, + self.param.reserve] = upk + return True + +class SetRobotParameters(RobotMessage): + def __init__(self): + self.param = robotParam + + def pack(self): + data = [self.param.wheel_diameter, + self.param.wheel_track, + self.param.encoder_resolution, + self.param.do_pid_interval, + self.param.kp, + self.param.ki, + self.param.kd, + self.param.ko, + self.param.cmd_last_time, + self.param.max_v_liner_x, + self.param.max_v_liner_y, + self.param.max_v_angular_z, + self.param.imu_type, + self.param.motor_ratio, + self.param.model_type, + self.param.motor_nonexchange_flag, + self.param.encoder_nonexchange_flag, + self.param.reserve] + + print(data) + pk = struct.pack('<3H1B8H1B1H3B%ds'%(64-(3*2+1+8*2+1+2+3)), *data) + return pk + + def unpack(self, data): + return True + +class RobotVel(RobotMessage): + def __init__(self): + self.v_liner_x = 0 + self.v_liner_y = 0 + self.v_angular_z = 0 + + def pack(self): + data = [self.v_liner_x, + self.v_liner_y, + self.v_angular_z] + pk = struct.pack('3h', *data) + return pk + + def unpack(self, data): + return True + +#todo the rest of the message classes +class RobotOdom(RobotMessage): + def __init__(self): + self.v_liner_x = 0 + self.v_liner_y = 0 + self.v_angular_z = 0 + self.x = 0 + self.y = 0 + self.yaw = 0 + + def unpack(self, data): + try: + upk = struct.unpack('<3H2l1H', bytes(data)) + except: + return False + [self.v_liner_x, + self.v_liner_y, + self.v_angular_z, + self.x, + self.y, + self.yaw] = upk + return True + +class RobotPIDData(RobotMessage): + def __init__(self): + self.pid_data = [0]*8 + + def unpack(self, data): + try: + upk = struct.unpack('<8l', bytes(data)) + except: + return False + + self.pid_data = upk + return True + +class RobotIMU(RobotMessage): + def __init__(self): + self.imu = [0]*9 + + def unpack(self, data): + try: + upk = struct.unpack('<9f', bytes(data)) + except: + return False + + self.imu = upk + return True + +class RobotEncoderCount(RobotMessage): + def __init__(self): + self.encoder = [0]*4 + + def unpack(self, data): + try: + upk = struct.unpack('<4f', bytes(data)) + except: + return False + + self.encoder = upk + return True + +class RobotMotorPWM(RobotMessage): + def __init__(self): + self.pwm = [0]*4 + + def pack(self): + pk = struct.pack('4h', *self.pwm) + return pk + + def unpack(self, data): + return True + +BoardDataDict = {MessageID.ID_GET_VERSION:RobotFirmwareInfo(), + MessageID.ID_GET_ROBOT_PARAMETER:GetRobotParameters(), + MessageID.ID_SET_ROBOT_PARAMETER:SetRobotParameters(), + MessageID.ID_SET_VEL:RobotVel(), + MessageID.ID_GET_ODOM:RobotOdom(), + MessageID.ID_GET_PID_DEBUG: RobotPIDData(), + MessageID.ID_GET_IMU: RobotIMU(), + MessageID.ID_GET_ENCODER_COUNT: RobotEncoderCount(), + MessageID.ID_SET_MOTOR_PWM: RobotMotorPWM(), + } + diff --git a/Hardware/UPbot-Tools/main.py b/Hardware/UPbot-Tools/main.py new file mode 100644 index 0000000..5eaeb3a --- /dev/null +++ b/Hardware/UPbot-Tools/main.py @@ -0,0 +1,115 @@ +import platform +import sys +sys.path.append("..") +import pypibot +from pypibot import log +from transport import Transport +from dataholder import MessageID +import params +import time +import signal + +#for linux +#port="/dev/pibot" + +#for windows +port="COM7" + +pypibot.assistant.enableGlobalExcept() +#log.enableFileLog(log_dir + "ros_$(Date8)_$(filenumber2).log") +log.setLevel("i") + +run_flag = True + +def exit(signum, frame): + global run_flag + run_flag = False + +if __name__ == '__main__': + signal.signal(signal.SIGINT, exit) + + mboard = Transport(port, params.pibotBaud) + if not mboard.start(): + log.error("can not open %s"%port) + sys.exit() + + DataHolder = mboard.getDataHolder() + + for num in range(0,3): + log.info("****************get robot version*****************") + boardVersion = DataHolder[MessageID.ID_GET_VERSION] + p = mboard.request(MessageID.ID_GET_VERSION) + if p: + log.info("firmware version:%s buildtime:%s\r\n"%(boardVersion.version.decode(), boardVersion.build_time.decode())) + break + else: + log.error('read firmware version err\r\n') + import time + time.sleep(1) + if num == 2: + log.error('please check connection or baudrate\r\n') + sys.exit() + + # get robot parameter + robotParam = DataHolder[MessageID.ID_GET_ROBOT_PARAMETER] + p = 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\r\n" \ + %(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\r\n" \ + %(robotParam.param.motor_nonexchange_flag,\ + robotParam.param.encoder_nonexchange_flag + )) + else: + log.error('get params err\r\n') + quit(1) + + log.info("****************get odom&imu*****************") + while run_flag: + robotOdom = DataHolder[MessageID.ID_GET_ODOM] + p = mboard.request(MessageID.ID_GET_ODOM) + if p: + log.info('request get odom success, vx=%d vy=%d vangular=%d, x=%d y=%d yaw=%d'%(robotOdom.v_liner_x, \ + robotOdom.v_liner_y, \ + robotOdom.v_angular_z, \ + robotOdom.x, \ + robotOdom.y, \ + robotOdom.yaw)) + else: + log.error('get odom err') + quit(1) + + robotIMU = DataHolder[MessageID.ID_GET_IMU].imu + p = mboard.request(MessageID.ID_GET_IMU) + if p: + log.info('get imu success, imu=[%f %f %f %f %f %f %f %f %f]'%(robotIMU[0], robotIMU[1], robotIMU[2], \ + robotIMU[3], robotIMU[4], robotIMU[5], \ + robotIMU[6], robotIMU[7], robotIMU[8])) + else: + log.error('get imu err') + quit(1) + + time.sleep(0.1) diff --git a/Hardware/UPbot-Tools/params.py b/Hardware/UPbot-Tools/params.py new file mode 100644 index 0000000..086ab3a --- /dev/null +++ b/Hardware/UPbot-Tools/params.py @@ -0,0 +1,82 @@ +import dataholder +import os +from dataholder import RobotImuType +from dataholder import RobotModelType + +# for linux +# pibotModel = os.environ['PIBOT_MODEL'] +# boardType = os.environ['PIBOT_BOARD'] +# pibotBaud = os.environ['PIBOT_DRIVER_BAUDRATE'] +# for windwos +pibotModel = "apollo" +boardType = "stm32f1" +pibotBaud = 115200 + +print(pibotModel) +print(boardType) +print(pibotBaud) + + + +pibotParam = dataholder.RobotParameters() + +if pibotModel == "apollo" and boardType == "arduino": + pibotParam = dataholder.RobotParameters(65, 175, 44, 10, \ + 75, 2500, 0, 10, \ + 250, 40, 0, 200, \ + RobotImuType.IMU_TYPE_GY85, 90, \ + RobotModelType.MODEL_TYPE_2WD_DIFF) +elif pibotModel == "apollo" and boardType == "stm32f1": + pibotParam = dataholder.RobotParameters(65, 175, 44, 10, \ + 320, 2700, 0, 10, \ + 250, 50, 0, 200, \ + RobotImuType.IMU_TYPE_GY87, 90, \ + RobotModelType.MODEL_TYPE_2WD_DIFF) +elif pibotModel == "apollo" and boardType == "stm32f4": + pibotParam = dataholder.RobotParameters(65, 175, 44, 10, \ + 320, 2700, 0, 10, \ + 250, 40, 0, 200, \ + RobotImuType.IMU_TYPE_GY87, 90, \ + RobotModelType.MODEL_TYPE_2WD_DIFF) +elif pibotModel == "zeus" and boardType == "stm32f4": + pibotParam = dataholder.RobotParameters(58, 230, 44, 10, \ + 320, 2700, 0, 10, \ + 250, 50, 50, 250, \ + RobotImuType.IMU_TYPE_GY87, 90, \ + RobotModelType.MODEL_TYPE_3WD_OMNI) +elif pibotModel == "hades" and boardType == "stm32f4": + pibotParam = dataholder.RobotParameters(76, 470, 44, 10, \ + 320, 2700, 0, 10, \ + 250, 50, 50, 250, \ + RobotImuType.IMU_TYPE_GY87, 90, \ + RobotModelType.MODEL_TYPE_4WD_MECANUM) +elif pibotModel == "hadesX" and boardType == "stm32f4": + pibotParam = dataholder.RobotParameters(150, 565, 44, 10, \ + 250, 2750, 0, 10, \ + 250, 50, 50, 250, \ + RobotImuType.IMU_TYPE_GY87, 72, \ + RobotModelType.MODEL_TYPE_4WD_MECANUM) +elif pibotModel == "hera" and boardType == "stm32f4": + pibotParam = dataholder.RobotParameters(82, 338, 44, 10, \ + 320, 2700, 0, 10, \ + 250, 50, 50, 250, \ + RobotImuType.IMU_TYPE_GY87, 90, \ + RobotModelType.MODEL_TYPE_4WD_DIFF) +elif pibotModel == "apolloX" and boardType == "arduino": + pibotParam = dataholder.RobotParameters(96, 350, 68, 10, \ + 75, 2500, 0, 10, \ + 250, 40, 0, 200, \ + RobotImuType.IMU_TYPE_GY85, 90, \ + RobotModelType.MODEL_TYPE_2WD_DIFF) +elif pibotModel == "apolloX" and boardType == "stm32f1": + pibotParam = dataholder.RobotParameters(96, 350, 68, 10, \ + 250, 1200, 0, 10, \ + 250, 50, 0, 200, \ + RobotImuType.IMU_TYPE_GY87, 90, \ + RobotModelType.MODEL_TYPE_2WD_DIFF) +elif pibotModel == "apolloX" and boardType == "stm32f4": + pibotParam = dataholder.RobotParameters(96, 350, 68, 10, \ + 250, 1200, 0, 10, \ + 250, 50, 0, 200, \ + RobotImuType.IMU_TYPE_GY87, 90, \ + RobotModelType.MODEL_TYPE_2WD_DIFF) \ No newline at end of file diff --git a/Hardware/UPbot-Tools/pb.py b/Hardware/UPbot-Tools/pb.py new file mode 100644 index 0000000..d989af2 --- /dev/null +++ b/Hardware/UPbot-Tools/pb.py @@ -0,0 +1,749 @@ +# -*- coding: utf-8 -*- + +# Form implementation generated from reading ui file 'pb.ui' +# +# Created by: PyQt5 UI code generator 5.15.9 +# +# WARNING: Any manual changes made to this file will be lost when pyuic5 is +# run again. Do not edit this file unless you know what you are doing. + + +from PyQt5 import QtCore, QtGui, QtWidgets + + +class Ui_pb(object): + def setupUi(self, pb): + pb.setObjectName("pb") + pb.resize(866, 542) + self.gridLayout_2 = QtWidgets.QGridLayout(pb) + self.gridLayout_2.setObjectName("gridLayout_2") + self.line_5 = QtWidgets.QFrame(pb) + self.line_5.setFrameShape(QtWidgets.QFrame.HLine) + self.line_5.setFrameShadow(QtWidgets.QFrame.Sunken) + self.line_5.setObjectName("line_5") + self.gridLayout_2.addWidget(self.line_5, 0, 0, 1, 1) + self.tabWidget = QtWidgets.QTabWidget(pb) + self.tabWidget.setObjectName("tabWidget") + self.tab_3 = QtWidgets.QWidget() + self.tab_3.setObjectName("tab_3") + self.verticalLayout_3 = QtWidgets.QVBoxLayout(self.tab_3) + self.verticalLayout_3.setObjectName("verticalLayout_3") + self.horizontalLayout_25 = QtWidgets.QHBoxLayout() + self.horizontalLayout_25.setObjectName("horizontalLayout_25") + self.horizontalLayout_5 = QtWidgets.QHBoxLayout() + self.horizontalLayout_5.setContentsMargins(10, -1, -1, -1) + self.horizontalLayout_5.setObjectName("horizontalLayout_5") + self.label_2 = QtWidgets.QLabel(self.tab_3) + self.label_2.setObjectName("label_2") + self.horizontalLayout_5.addWidget(self.label_2) + spacerItem = QtWidgets.QSpacerItem(40, 20, QtWidgets.QSizePolicy.Expanding, QtWidgets.QSizePolicy.Minimum) + self.horizontalLayout_5.addItem(spacerItem) + self.combox_model = QtWidgets.QComboBox(self.tab_3) + self.combox_model.setObjectName("combox_model") + self.horizontalLayout_5.addWidget(self.combox_model) + spacerItem1 = QtWidgets.QSpacerItem(40, 20, QtWidgets.QSizePolicy.Expanding, QtWidgets.QSizePolicy.Minimum) + self.horizontalLayout_5.addItem(spacerItem1) + self.horizontalLayout_5.setStretch(1, 1) + self.horizontalLayout_5.setStretch(3, 8) + self.horizontalLayout_25.addLayout(self.horizontalLayout_5) + self.horizontalLayout_6 = QtWidgets.QHBoxLayout() + self.horizontalLayout_6.setContentsMargins(10, -1, -1, -1) + self.horizontalLayout_6.setObjectName("horizontalLayout_6") + self.label_3 = QtWidgets.QLabel(self.tab_3) + self.label_3.setObjectName("label_3") + self.horizontalLayout_6.addWidget(self.label_3) + spacerItem2 = QtWidgets.QSpacerItem(40, 20, QtWidgets.QSizePolicy.Expanding, QtWidgets.QSizePolicy.Minimum) + self.horizontalLayout_6.addItem(spacerItem2) + self.combox_imu_type = QtWidgets.QComboBox(self.tab_3) + self.combox_imu_type.setObjectName("combox_imu_type") + self.horizontalLayout_6.addWidget(self.combox_imu_type) + spacerItem3 = QtWidgets.QSpacerItem(40, 20, QtWidgets.QSizePolicy.Expanding, QtWidgets.QSizePolicy.Minimum) + self.horizontalLayout_6.addItem(spacerItem3) + self.horizontalLayout_6.setStretch(1, 1) + self.horizontalLayout_6.setStretch(2, 8) + self.horizontalLayout_6.setStretch(3, 8) + self.horizontalLayout_25.addLayout(self.horizontalLayout_6) + self.verticalLayout_3.addLayout(self.horizontalLayout_25) + self.line_2 = QtWidgets.QFrame(self.tab_3) + self.line_2.setFrameShape(QtWidgets.QFrame.HLine) + self.line_2.setFrameShadow(QtWidgets.QFrame.Sunken) + self.line_2.setObjectName("line_2") + self.verticalLayout_3.addWidget(self.line_2) + self.verticalLayout_2 = QtWidgets.QVBoxLayout() + self.verticalLayout_2.setObjectName("verticalLayout_2") + self.gridLayout = QtWidgets.QGridLayout() + self.gridLayout.setObjectName("gridLayout") + self.horizontalLayout = QtWidgets.QHBoxLayout() + self.horizontalLayout.setContentsMargins(10, -1, 10, -1) + self.horizontalLayout.setObjectName("horizontalLayout") + self.label_6 = QtWidgets.QLabel(self.tab_3) + self.label_6.setObjectName("label_6") + self.horizontalLayout.addWidget(self.label_6) + self.slider_motor_ratio = QtWidgets.QSlider(self.tab_3) + self.slider_motor_ratio.setOrientation(QtCore.Qt.Horizontal) + self.slider_motor_ratio.setObjectName("slider_motor_ratio") + self.horizontalLayout.addWidget(self.slider_motor_ratio) + self.label_motor_ratio = QtWidgets.QLabel(self.tab_3) + self.label_motor_ratio.setObjectName("label_motor_ratio") + self.horizontalLayout.addWidget(self.label_motor_ratio) + self.gridLayout.addLayout(self.horizontalLayout, 1, 1, 1, 1) + self.horizontalLayout_3 = QtWidgets.QHBoxLayout() + self.horizontalLayout_3.setContentsMargins(10, -1, 10, -1) + self.horizontalLayout_3.setObjectName("horizontalLayout_3") + self.label = QtWidgets.QLabel(self.tab_3) + self.label.setObjectName("label") + self.horizontalLayout_3.addWidget(self.label) + self.slider_wheel_diameter = QtWidgets.QSlider(self.tab_3) + self.slider_wheel_diameter.setOrientation(QtCore.Qt.Horizontal) + self.slider_wheel_diameter.setObjectName("slider_wheel_diameter") + self.horizontalLayout_3.addWidget(self.slider_wheel_diameter) + self.label_wheel_diameter = QtWidgets.QLabel(self.tab_3) + self.label_wheel_diameter.setObjectName("label_wheel_diameter") + self.horizontalLayout_3.addWidget(self.label_wheel_diameter) + self.horizontalLayout_3.setStretch(1, 16) + self.gridLayout.addLayout(self.horizontalLayout_3, 0, 0, 1, 1) + self.horizontalLayout_4 = QtWidgets.QHBoxLayout() + self.horizontalLayout_4.setContentsMargins(10, -1, 10, -1) + self.horizontalLayout_4.setObjectName("horizontalLayout_4") + self.label_4 = QtWidgets.QLabel(self.tab_3) + self.label_4.setObjectName("label_4") + self.horizontalLayout_4.addWidget(self.label_4) + self.slider_encoder = QtWidgets.QSlider(self.tab_3) + self.slider_encoder.setOrientation(QtCore.Qt.Horizontal) + self.slider_encoder.setObjectName("slider_encoder") + self.horizontalLayout_4.addWidget(self.slider_encoder) + self.label_encoder_res = QtWidgets.QLabel(self.tab_3) + self.label_encoder_res.setObjectName("label_encoder_res") + self.horizontalLayout_4.addWidget(self.label_encoder_res) + self.horizontalLayout_4.setStretch(1, 16) + self.gridLayout.addLayout(self.horizontalLayout_4, 1, 0, 1, 1) + self.horizontalLayout_2 = QtWidgets.QHBoxLayout() + self.horizontalLayout_2.setContentsMargins(10, -1, 10, -1) + self.horizontalLayout_2.setObjectName("horizontalLayout_2") + self.label_5 = QtWidgets.QLabel(self.tab_3) + self.label_5.setObjectName("label_5") + self.horizontalLayout_2.addWidget(self.label_5) + self.slider_wheel_track = QtWidgets.QSlider(self.tab_3) + self.slider_wheel_track.setOrientation(QtCore.Qt.Horizontal) + self.slider_wheel_track.setObjectName("slider_wheel_track") + self.horizontalLayout_2.addWidget(self.slider_wheel_track) + self.label_wheel_track = QtWidgets.QLabel(self.tab_3) + self.label_wheel_track.setObjectName("label_wheel_track") + self.horizontalLayout_2.addWidget(self.label_wheel_track) + self.gridLayout.addLayout(self.horizontalLayout_2, 0, 1, 1, 1) + self.verticalLayout_2.addLayout(self.gridLayout) + self.verticalLayout_3.addLayout(self.verticalLayout_2) + self.line_3 = QtWidgets.QFrame(self.tab_3) + self.line_3.setFrameShape(QtWidgets.QFrame.HLine) + self.line_3.setFrameShadow(QtWidgets.QFrame.Sunken) + self.line_3.setObjectName("line_3") + self.verticalLayout_3.addWidget(self.line_3) + self.verticalLayout = QtWidgets.QVBoxLayout() + self.verticalLayout.setObjectName("verticalLayout") + self.groupBox = QtWidgets.QGroupBox(self.tab_3) + self.groupBox.setAutoFillBackground(False) + self.groupBox.setFlat(False) + self.groupBox.setObjectName("groupBox") + self.verticalLayout_4 = QtWidgets.QVBoxLayout(self.groupBox) + self.verticalLayout_4.setObjectName("verticalLayout_4") + self.horizontalLayout_26 = QtWidgets.QHBoxLayout() + self.horizontalLayout_26.setObjectName("horizontalLayout_26") + self.gridLayout_4 = QtWidgets.QGridLayout() + self.gridLayout_4.setContentsMargins(5, -1, -1, -1) + self.gridLayout_4.setObjectName("gridLayout_4") + self.checkBox_motor1 = QtWidgets.QCheckBox(self.groupBox) + self.checkBox_motor1.setObjectName("checkBox_motor1") + self.gridLayout_4.addWidget(self.checkBox_motor1, 0, 0, 1, 1) + self.checkBox_motor2 = QtWidgets.QCheckBox(self.groupBox) + self.checkBox_motor2.setObjectName("checkBox_motor2") + self.gridLayout_4.addWidget(self.checkBox_motor2, 0, 1, 1, 1) + self.checkBox_motor3 = QtWidgets.QCheckBox(self.groupBox) + self.checkBox_motor3.setObjectName("checkBox_motor3") + self.gridLayout_4.addWidget(self.checkBox_motor3, 0, 2, 1, 1) + self.checkBox_motor4 = QtWidgets.QCheckBox(self.groupBox) + self.checkBox_motor4.setObjectName("checkBox_motor4") + self.gridLayout_4.addWidget(self.checkBox_motor4, 0, 3, 1, 1) + self.checkBox_encoder1 = QtWidgets.QCheckBox(self.groupBox) + self.checkBox_encoder1.setObjectName("checkBox_encoder1") + self.gridLayout_4.addWidget(self.checkBox_encoder1, 0, 4, 1, 1) + self.checkBox_encoder2 = QtWidgets.QCheckBox(self.groupBox) + self.checkBox_encoder2.setObjectName("checkBox_encoder2") + self.gridLayout_4.addWidget(self.checkBox_encoder2, 0, 5, 1, 1) + self.checkBox_encoder3 = QtWidgets.QCheckBox(self.groupBox) + self.checkBox_encoder3.setObjectName("checkBox_encoder3") + self.gridLayout_4.addWidget(self.checkBox_encoder3, 0, 6, 1, 1) + self.checkBox_encoder4 = QtWidgets.QCheckBox(self.groupBox) + self.checkBox_encoder4.setObjectName("checkBox_encoder4") + self.gridLayout_4.addWidget(self.checkBox_encoder4, 0, 7, 1, 1) + self.horizontalLayout_26.addLayout(self.gridLayout_4) + self.verticalLayout_4.addLayout(self.horizontalLayout_26) + self.verticalLayout.addWidget(self.groupBox) + self.verticalLayout_3.addLayout(self.verticalLayout) + self.horizontalLayout_24 = QtWidgets.QHBoxLayout() + self.horizontalLayout_24.setObjectName("horizontalLayout_24") + self.groupBox_3 = QtWidgets.QGroupBox(self.tab_3) + self.groupBox_3.setObjectName("groupBox_3") + self.gridLayout_5 = QtWidgets.QGridLayout(self.groupBox_3) + self.gridLayout_5.setObjectName("gridLayout_5") + self.horizontalLayout_20 = QtWidgets.QHBoxLayout() + self.horizontalLayout_20.setContentsMargins(10, -1, -1, -1) + self.horizontalLayout_20.setObjectName("horizontalLayout_20") + self.label_16 = QtWidgets.QLabel(self.groupBox_3) + self.label_16.setObjectName("label_16") + self.horizontalLayout_20.addWidget(self.label_16) + self.slider_ki = QtWidgets.QSlider(self.groupBox_3) + self.slider_ki.setOrientation(QtCore.Qt.Horizontal) + self.slider_ki.setObjectName("slider_ki") + self.horizontalLayout_20.addWidget(self.slider_ki) + self.label_ki = QtWidgets.QLabel(self.groupBox_3) + self.label_ki.setObjectName("label_ki") + self.horizontalLayout_20.addWidget(self.label_ki) + self.gridLayout_5.addLayout(self.horizontalLayout_20, 2, 0, 1, 1) + self.horizontalLayout_21 = QtWidgets.QHBoxLayout() + self.horizontalLayout_21.setContentsMargins(10, -1, -1, -1) + self.horizontalLayout_21.setObjectName("horizontalLayout_21") + self.label_17 = QtWidgets.QLabel(self.groupBox_3) + self.label_17.setObjectName("label_17") + self.horizontalLayout_21.addWidget(self.label_17) + self.slider_kd = QtWidgets.QSlider(self.groupBox_3) + self.slider_kd.setOrientation(QtCore.Qt.Horizontal) + self.slider_kd.setObjectName("slider_kd") + self.horizontalLayout_21.addWidget(self.slider_kd) + self.label_kd = QtWidgets.QLabel(self.groupBox_3) + self.label_kd.setObjectName("label_kd") + self.horizontalLayout_21.addWidget(self.label_kd) + self.gridLayout_5.addLayout(self.horizontalLayout_21, 3, 0, 1, 1) + self.horizontalLayout_22 = QtWidgets.QHBoxLayout() + self.horizontalLayout_22.setContentsMargins(10, -1, -1, -1) + self.horizontalLayout_22.setObjectName("horizontalLayout_22") + self.label_15 = QtWidgets.QLabel(self.groupBox_3) + self.label_15.setObjectName("label_15") + self.horizontalLayout_22.addWidget(self.label_15) + self.slider_kp = QtWidgets.QSlider(self.groupBox_3) + self.slider_kp.setOrientation(QtCore.Qt.Horizontal) + self.slider_kp.setObjectName("slider_kp") + self.horizontalLayout_22.addWidget(self.slider_kp) + self.label_kp = QtWidgets.QLabel(self.groupBox_3) + self.label_kp.setObjectName("label_kp") + self.horizontalLayout_22.addWidget(self.label_kp) + self.gridLayout_5.addLayout(self.horizontalLayout_22, 1, 0, 1, 1) + self.horizontalLayout_23 = QtWidgets.QHBoxLayout() + self.horizontalLayout_23.setContentsMargins(10, -1, -1, -1) + self.horizontalLayout_23.setObjectName("horizontalLayout_23") + self.label_18 = QtWidgets.QLabel(self.groupBox_3) + self.label_18.setObjectName("label_18") + self.horizontalLayout_23.addWidget(self.label_18) + self.slider_ko = QtWidgets.QSlider(self.groupBox_3) + self.slider_ko.setOrientation(QtCore.Qt.Horizontal) + self.slider_ko.setObjectName("slider_ko") + self.horizontalLayout_23.addWidget(self.slider_ko) + self.label_ko = QtWidgets.QLabel(self.groupBox_3) + self.label_ko.setObjectName("label_ko") + self.horizontalLayout_23.addWidget(self.label_ko) + self.gridLayout_5.addLayout(self.horizontalLayout_23, 4, 0, 1, 1) + self.horizontalLayout_15 = QtWidgets.QHBoxLayout() + self.horizontalLayout_15.setContentsMargins(10, -1, -1, -1) + self.horizontalLayout_15.setObjectName("horizontalLayout_15") + self.label_19 = QtWidgets.QLabel(self.groupBox_3) + self.label_19.setObjectName("label_19") + self.horizontalLayout_15.addWidget(self.label_19) + self.slider_pid_interval = QtWidgets.QSlider(self.groupBox_3) + self.slider_pid_interval.setOrientation(QtCore.Qt.Horizontal) + self.slider_pid_interval.setObjectName("slider_pid_interval") + self.horizontalLayout_15.addWidget(self.slider_pid_interval) + self.label_pid_interval = QtWidgets.QLabel(self.groupBox_3) + self.label_pid_interval.setObjectName("label_pid_interval") + self.horizontalLayout_15.addWidget(self.label_pid_interval) + self.gridLayout_5.addLayout(self.horizontalLayout_15, 0, 0, 1, 1) + self.horizontalLayout_24.addWidget(self.groupBox_3) + self.groupBox_2 = QtWidgets.QGroupBox(self.tab_3) + self.groupBox_2.setObjectName("groupBox_2") + self.gridLayout_3 = QtWidgets.QGridLayout(self.groupBox_2) + self.gridLayout_3.setObjectName("gridLayout_3") + self.horizontalLayout_17 = QtWidgets.QHBoxLayout() + self.horizontalLayout_17.setContentsMargins(10, -1, -1, -1) + self.horizontalLayout_17.setObjectName("horizontalLayout_17") + self.label_22 = QtWidgets.QLabel(self.groupBox_2) + self.label_22.setObjectName("label_22") + self.horizontalLayout_17.addWidget(self.label_22) + self.slider_vy_max = QtWidgets.QSlider(self.groupBox_2) + self.slider_vy_max.setOrientation(QtCore.Qt.Horizontal) + self.slider_vy_max.setObjectName("slider_vy_max") + self.horizontalLayout_17.addWidget(self.slider_vy_max) + self.label_vy_max = QtWidgets.QLabel(self.groupBox_2) + self.label_vy_max.setObjectName("label_vy_max") + self.horizontalLayout_17.addWidget(self.label_vy_max) + self.gridLayout_3.addLayout(self.horizontalLayout_17, 3, 0, 1, 1) + self.horizontalLayout_18 = QtWidgets.QHBoxLayout() + self.horizontalLayout_18.setContentsMargins(10, -1, -1, -1) + self.horizontalLayout_18.setObjectName("horizontalLayout_18") + self.label_21 = QtWidgets.QLabel(self.groupBox_2) + self.label_21.setObjectName("label_21") + self.horizontalLayout_18.addWidget(self.label_21) + self.slider_vx_max = QtWidgets.QSlider(self.groupBox_2) + self.slider_vx_max.setOrientation(QtCore.Qt.Horizontal) + self.slider_vx_max.setObjectName("slider_vx_max") + self.horizontalLayout_18.addWidget(self.slider_vx_max) + self.label_vx_max = QtWidgets.QLabel(self.groupBox_2) + self.label_vx_max.setObjectName("label_vx_max") + self.horizontalLayout_18.addWidget(self.label_vx_max) + self.gridLayout_3.addLayout(self.horizontalLayout_18, 2, 0, 1, 1) + self.horizontalLayout_19 = QtWidgets.QHBoxLayout() + self.horizontalLayout_19.setContentsMargins(10, -1, -1, -1) + self.horizontalLayout_19.setObjectName("horizontalLayout_19") + self.label_23 = QtWidgets.QLabel(self.groupBox_2) + self.label_23.setObjectName("label_23") + self.horizontalLayout_19.addWidget(self.label_23) + self.slider_va_max = QtWidgets.QSlider(self.groupBox_2) + self.slider_va_max.setOrientation(QtCore.Qt.Horizontal) + self.slider_va_max.setObjectName("slider_va_max") + self.horizontalLayout_19.addWidget(self.slider_va_max) + self.label_va_max = QtWidgets.QLabel(self.groupBox_2) + self.label_va_max.setObjectName("label_va_max") + self.horizontalLayout_19.addWidget(self.label_va_max) + self.gridLayout_3.addLayout(self.horizontalLayout_19, 4, 0, 1, 1) + self.horizontalLayout_16 = QtWidgets.QHBoxLayout() + self.horizontalLayout_16.setContentsMargins(10, -1, -1, -1) + self.horizontalLayout_16.setObjectName("horizontalLayout_16") + self.label_20 = QtWidgets.QLabel(self.groupBox_2) + self.label_20.setObjectName("label_20") + self.horizontalLayout_16.addWidget(self.label_20) + self.slider_cmd_lasttime = QtWidgets.QSlider(self.groupBox_2) + self.slider_cmd_lasttime.setOrientation(QtCore.Qt.Horizontal) + self.slider_cmd_lasttime.setObjectName("slider_cmd_lasttime") + self.horizontalLayout_16.addWidget(self.slider_cmd_lasttime) + self.label_cmd_lasttime = QtWidgets.QLabel(self.groupBox_2) + self.label_cmd_lasttime.setObjectName("label_cmd_lasttime") + self.horizontalLayout_16.addWidget(self.label_cmd_lasttime) + self.gridLayout_3.addLayout(self.horizontalLayout_16, 1, 0, 1, 1) + self.horizontalLayout_24.addWidget(self.groupBox_2) + self.verticalLayout_3.addLayout(self.horizontalLayout_24) + self.line_4 = QtWidgets.QFrame(self.tab_3) + self.line_4.setFrameShape(QtWidgets.QFrame.HLine) + self.line_4.setFrameShadow(QtWidgets.QFrame.Sunken) + self.line_4.setObjectName("line_4") + self.verticalLayout_3.addWidget(self.line_4) + self.horizontalLayout_29 = QtWidgets.QHBoxLayout() + self.horizontalLayout_29.setObjectName("horizontalLayout_29") + spacerItem4 = QtWidgets.QSpacerItem(40, 20, QtWidgets.QSizePolicy.Expanding, QtWidgets.QSizePolicy.Minimum) + self.horizontalLayout_29.addItem(spacerItem4) + self.comboBox_support_model = QtWidgets.QComboBox(self.tab_3) + self.comboBox_support_model.setObjectName("comboBox_support_model") + self.horizontalLayout_29.addWidget(self.comboBox_support_model) + spacerItem5 = QtWidgets.QSpacerItem(40, 20, QtWidgets.QSizePolicy.Expanding, QtWidgets.QSizePolicy.Minimum) + self.horizontalLayout_29.addItem(spacerItem5) + self.pushButton_load = QtWidgets.QPushButton(self.tab_3) + self.pushButton_load.setObjectName("pushButton_load") + self.horizontalLayout_29.addWidget(self.pushButton_load) + spacerItem6 = QtWidgets.QSpacerItem(40, 20, QtWidgets.QSizePolicy.Expanding, QtWidgets.QSizePolicy.Minimum) + self.horizontalLayout_29.addItem(spacerItem6) + self.pushButton_set = QtWidgets.QPushButton(self.tab_3) + self.pushButton_set.setObjectName("pushButton_set") + self.horizontalLayout_29.addWidget(self.pushButton_set) + spacerItem7 = QtWidgets.QSpacerItem(40, 20, QtWidgets.QSizePolicy.Expanding, QtWidgets.QSizePolicy.Minimum) + self.horizontalLayout_29.addItem(spacerItem7) + self.pushButton_read = QtWidgets.QPushButton(self.tab_3) + self.pushButton_read.setObjectName("pushButton_read") + self.horizontalLayout_29.addWidget(self.pushButton_read) + spacerItem8 = QtWidgets.QSpacerItem(40, 20, QtWidgets.QSizePolicy.Expanding, QtWidgets.QSizePolicy.Minimum) + self.horizontalLayout_29.addItem(spacerItem8) + self.horizontalLayout_29.setStretch(0, 20) + self.horizontalLayout_29.setStretch(1, 3) + self.horizontalLayout_29.setStretch(2, 1) + self.horizontalLayout_29.setStretch(3, 3) + self.horizontalLayout_29.setStretch(4, 1) + self.horizontalLayout_29.setStretch(5, 3) + self.horizontalLayout_29.setStretch(6, 1) + self.horizontalLayout_29.setStretch(7, 3) + self.horizontalLayout_29.setStretch(8, 1) + self.verticalLayout_3.addLayout(self.horizontalLayout_29) + self.tabWidget.addTab(self.tab_3, "") + self.tab_4 = QtWidgets.QWidget() + self.tab_4.setObjectName("tab_4") + self.verticalLayout_6 = QtWidgets.QVBoxLayout(self.tab_4) + self.verticalLayout_6.setObjectName("verticalLayout_6") + self.groupBox_4 = QtWidgets.QGroupBox(self.tab_4) + self.groupBox_4.setObjectName("groupBox_4") + self.verticalLayout_5 = QtWidgets.QVBoxLayout(self.groupBox_4) + self.verticalLayout_5.setObjectName("verticalLayout_5") + self.horizontalLayout_7 = QtWidgets.QHBoxLayout() + self.horizontalLayout_7.setObjectName("horizontalLayout_7") + self.label_7 = QtWidgets.QLabel(self.groupBox_4) + self.label_7.setAlignment(QtCore.Qt.AlignCenter) + self.label_7.setObjectName("label_7") + self.horizontalLayout_7.addWidget(self.label_7) + self.slider_set_pwm1 = QtWidgets.QSlider(self.groupBox_4) + self.slider_set_pwm1.setOrientation(QtCore.Qt.Horizontal) + self.slider_set_pwm1.setObjectName("slider_set_pwm1") + self.horizontalLayout_7.addWidget(self.slider_set_pwm1) + self.label_set_pwm1 = QtWidgets.QLabel(self.groupBox_4) + self.label_set_pwm1.setAlignment(QtCore.Qt.AlignCenter) + self.label_set_pwm1.setObjectName("label_set_pwm1") + self.horizontalLayout_7.addWidget(self.label_set_pwm1) + spacerItem9 = QtWidgets.QSpacerItem(40, 20, QtWidgets.QSizePolicy.Expanding, QtWidgets.QSizePolicy.Minimum) + self.horizontalLayout_7.addItem(spacerItem9) + self.label_10 = QtWidgets.QLabel(self.groupBox_4) + self.label_10.setObjectName("label_10") + self.horizontalLayout_7.addWidget(self.label_10) + self.label_feedback1 = QtWidgets.QLabel(self.groupBox_4) + self.label_feedback1.setAlignment(QtCore.Qt.AlignCenter) + self.label_feedback1.setObjectName("label_feedback1") + self.horizontalLayout_7.addWidget(self.label_feedback1) + self.horizontalLayout_7.setStretch(0, 1) + self.horizontalLayout_7.setStretch(1, 10) + self.horizontalLayout_7.setStretch(2, 1) + self.horizontalLayout_7.setStretch(3, 2) + self.horizontalLayout_7.setStretch(4, 1) + self.horizontalLayout_7.setStretch(5, 1) + self.verticalLayout_5.addLayout(self.horizontalLayout_7) + self.horizontalLayout_8 = QtWidgets.QHBoxLayout() + self.horizontalLayout_8.setObjectName("horizontalLayout_8") + self.label_8 = QtWidgets.QLabel(self.groupBox_4) + self.label_8.setAlignment(QtCore.Qt.AlignCenter) + self.label_8.setObjectName("label_8") + self.horizontalLayout_8.addWidget(self.label_8) + self.slider_set_pwm2 = QtWidgets.QSlider(self.groupBox_4) + self.slider_set_pwm2.setOrientation(QtCore.Qt.Horizontal) + self.slider_set_pwm2.setObjectName("slider_set_pwm2") + self.horizontalLayout_8.addWidget(self.slider_set_pwm2) + self.label_set_pwm2 = QtWidgets.QLabel(self.groupBox_4) + self.label_set_pwm2.setAlignment(QtCore.Qt.AlignCenter) + self.label_set_pwm2.setObjectName("label_set_pwm2") + self.horizontalLayout_8.addWidget(self.label_set_pwm2) + spacerItem10 = QtWidgets.QSpacerItem(40, 20, QtWidgets.QSizePolicy.Expanding, QtWidgets.QSizePolicy.Minimum) + self.horizontalLayout_8.addItem(spacerItem10) + self.label_11 = QtWidgets.QLabel(self.groupBox_4) + self.label_11.setObjectName("label_11") + self.horizontalLayout_8.addWidget(self.label_11) + self.label_feedback2 = QtWidgets.QLabel(self.groupBox_4) + self.label_feedback2.setAlignment(QtCore.Qt.AlignCenter) + self.label_feedback2.setObjectName("label_feedback2") + self.horizontalLayout_8.addWidget(self.label_feedback2) + self.horizontalLayout_8.setStretch(0, 1) + self.horizontalLayout_8.setStretch(1, 10) + self.horizontalLayout_8.setStretch(2, 1) + self.horizontalLayout_8.setStretch(3, 2) + self.horizontalLayout_8.setStretch(4, 1) + self.horizontalLayout_8.setStretch(5, 1) + self.verticalLayout_5.addLayout(self.horizontalLayout_8) + self.horizontalLayout_9 = QtWidgets.QHBoxLayout() + self.horizontalLayout_9.setObjectName("horizontalLayout_9") + self.label_9 = QtWidgets.QLabel(self.groupBox_4) + self.label_9.setAlignment(QtCore.Qt.AlignCenter) + self.label_9.setObjectName("label_9") + self.horizontalLayout_9.addWidget(self.label_9) + self.slider_set_pwm3 = QtWidgets.QSlider(self.groupBox_4) + self.slider_set_pwm3.setOrientation(QtCore.Qt.Horizontal) + self.slider_set_pwm3.setObjectName("slider_set_pwm3") + self.horizontalLayout_9.addWidget(self.slider_set_pwm3) + self.label_set_pwm3 = QtWidgets.QLabel(self.groupBox_4) + self.label_set_pwm3.setAlignment(QtCore.Qt.AlignCenter) + self.label_set_pwm3.setObjectName("label_set_pwm3") + self.horizontalLayout_9.addWidget(self.label_set_pwm3) + spacerItem11 = QtWidgets.QSpacerItem(40, 20, QtWidgets.QSizePolicy.Expanding, QtWidgets.QSizePolicy.Minimum) + self.horizontalLayout_9.addItem(spacerItem11) + self.label_12 = QtWidgets.QLabel(self.groupBox_4) + self.label_12.setObjectName("label_12") + self.horizontalLayout_9.addWidget(self.label_12) + self.label_feedback3 = QtWidgets.QLabel(self.groupBox_4) + self.label_feedback3.setAlignment(QtCore.Qt.AlignCenter) + self.label_feedback3.setObjectName("label_feedback3") + self.horizontalLayout_9.addWidget(self.label_feedback3) + self.horizontalLayout_9.setStretch(0, 1) + self.horizontalLayout_9.setStretch(1, 10) + self.horizontalLayout_9.setStretch(2, 1) + self.horizontalLayout_9.setStretch(3, 2) + self.horizontalLayout_9.setStretch(4, 1) + self.horizontalLayout_9.setStretch(5, 1) + self.verticalLayout_5.addLayout(self.horizontalLayout_9) + self.horizontalLayout_10 = QtWidgets.QHBoxLayout() + self.horizontalLayout_10.setObjectName("horizontalLayout_10") + self.label_13 = QtWidgets.QLabel(self.groupBox_4) + self.label_13.setAlignment(QtCore.Qt.AlignCenter) + self.label_13.setObjectName("label_13") + self.horizontalLayout_10.addWidget(self.label_13) + self.slider_set_pwm4 = QtWidgets.QSlider(self.groupBox_4) + self.slider_set_pwm4.setOrientation(QtCore.Qt.Horizontal) + self.slider_set_pwm4.setObjectName("slider_set_pwm4") + self.horizontalLayout_10.addWidget(self.slider_set_pwm4) + self.label_set_pwm4 = QtWidgets.QLabel(self.groupBox_4) + self.label_set_pwm4.setAlignment(QtCore.Qt.AlignCenter) + self.label_set_pwm4.setObjectName("label_set_pwm4") + self.horizontalLayout_10.addWidget(self.label_set_pwm4) + spacerItem12 = QtWidgets.QSpacerItem(40, 20, QtWidgets.QSizePolicy.Expanding, QtWidgets.QSizePolicy.Minimum) + self.horizontalLayout_10.addItem(spacerItem12) + self.label_14 = QtWidgets.QLabel(self.groupBox_4) + self.label_14.setObjectName("label_14") + self.horizontalLayout_10.addWidget(self.label_14) + self.label_feedback4 = QtWidgets.QLabel(self.groupBox_4) + self.label_feedback4.setAlignment(QtCore.Qt.AlignCenter) + self.label_feedback4.setObjectName("label_feedback4") + self.horizontalLayout_10.addWidget(self.label_feedback4) + self.horizontalLayout_10.setStretch(1, 10) + self.horizontalLayout_10.setStretch(2, 1) + self.horizontalLayout_10.setStretch(3, 2) + self.horizontalLayout_10.setStretch(4, 1) + self.horizontalLayout_10.setStretch(5, 1) + self.verticalLayout_5.addLayout(self.horizontalLayout_10) + self.horizontalLayout_11 = QtWidgets.QHBoxLayout() + self.horizontalLayout_11.setObjectName("horizontalLayout_11") + spacerItem13 = QtWidgets.QSpacerItem(40, 20, QtWidgets.QSizePolicy.Expanding, QtWidgets.QSizePolicy.Minimum) + self.horizontalLayout_11.addItem(spacerItem13) + self.pushButton_start = QtWidgets.QPushButton(self.groupBox_4) + self.pushButton_start.setObjectName("pushButton_start") + self.horizontalLayout_11.addWidget(self.pushButton_start) + self.pushButton_stop = QtWidgets.QPushButton(self.groupBox_4) + self.pushButton_stop.setObjectName("pushButton_stop") + self.horizontalLayout_11.addWidget(self.pushButton_stop) + self.verticalLayout_5.addLayout(self.horizontalLayout_11) + self.verticalLayout_6.addWidget(self.groupBox_4) + self.groupBox_6 = QtWidgets.QGroupBox(self.tab_4) + self.groupBox_6.setObjectName("groupBox_6") + self.verticalLayout_7 = QtWidgets.QVBoxLayout(self.groupBox_6) + self.verticalLayout_7.setObjectName("verticalLayout_7") + self.horizontalLayout_13 = QtWidgets.QHBoxLayout() + self.horizontalLayout_13.setObjectName("horizontalLayout_13") + self.label_25 = QtWidgets.QLabel(self.groupBox_6) + self.label_25.setAlignment(QtCore.Qt.AlignCenter) + self.label_25.setObjectName("label_25") + self.horizontalLayout_13.addWidget(self.label_25) + self.label_input_1 = QtWidgets.QLabel(self.groupBox_6) + self.label_input_1.setAlignment(QtCore.Qt.AlignCenter) + self.label_input_1.setObjectName("label_input_1") + self.horizontalLayout_13.addWidget(self.label_input_1) + self.label_input_2 = QtWidgets.QLabel(self.groupBox_6) + self.label_input_2.setAlignment(QtCore.Qt.AlignCenter) + self.label_input_2.setObjectName("label_input_2") + self.horizontalLayout_13.addWidget(self.label_input_2) + self.label_input_3 = QtWidgets.QLabel(self.groupBox_6) + self.label_input_3.setAlignment(QtCore.Qt.AlignCenter) + self.label_input_3.setObjectName("label_input_3") + self.horizontalLayout_13.addWidget(self.label_input_3) + self.label_input_4 = QtWidgets.QLabel(self.groupBox_6) + self.label_input_4.setAlignment(QtCore.Qt.AlignCenter) + self.label_input_4.setObjectName("label_input_4") + self.horizontalLayout_13.addWidget(self.label_input_4) + self.verticalLayout_7.addLayout(self.horizontalLayout_13) + self.horizontalLayout_14 = QtWidgets.QHBoxLayout() + self.horizontalLayout_14.setObjectName("horizontalLayout_14") + self.label_27 = QtWidgets.QLabel(self.groupBox_6) + self.label_27.setAlignment(QtCore.Qt.AlignCenter) + self.label_27.setObjectName("label_27") + self.horizontalLayout_14.addWidget(self.label_27) + self.label_output_1 = QtWidgets.QLabel(self.groupBox_6) + self.label_output_1.setAlignment(QtCore.Qt.AlignCenter) + self.label_output_1.setObjectName("label_output_1") + self.horizontalLayout_14.addWidget(self.label_output_1) + self.label_output_2 = QtWidgets.QLabel(self.groupBox_6) + self.label_output_2.setAlignment(QtCore.Qt.AlignCenter) + self.label_output_2.setObjectName("label_output_2") + self.horizontalLayout_14.addWidget(self.label_output_2) + self.label_output_3 = QtWidgets.QLabel(self.groupBox_6) + self.label_output_3.setAlignment(QtCore.Qt.AlignCenter) + self.label_output_3.setObjectName("label_output_3") + self.horizontalLayout_14.addWidget(self.label_output_3) + self.label_output_4 = QtWidgets.QLabel(self.groupBox_6) + self.label_output_4.setAlignment(QtCore.Qt.AlignCenter) + self.label_output_4.setObjectName("label_output_4") + self.horizontalLayout_14.addWidget(self.label_output_4) + self.verticalLayout_7.addLayout(self.horizontalLayout_14) + self.horizontalLayout_27 = QtWidgets.QHBoxLayout() + self.horizontalLayout_27.setObjectName("horizontalLayout_27") + self.label_29 = QtWidgets.QLabel(self.groupBox_6) + self.label_29.setAlignment(QtCore.Qt.AlignCenter) + self.label_29.setObjectName("label_29") + self.horizontalLayout_27.addWidget(self.label_29) + self.label_30 = QtWidgets.QLabel(self.groupBox_6) + self.label_30.setAlignment(QtCore.Qt.AlignCenter) + self.label_30.setObjectName("label_30") + self.horizontalLayout_27.addWidget(self.label_30) + self.label_31 = QtWidgets.QLabel(self.groupBox_6) + self.label_31.setAlignment(QtCore.Qt.AlignCenter) + self.label_31.setObjectName("label_31") + self.horizontalLayout_27.addWidget(self.label_31) + self.pushButton_start_2 = QtWidgets.QPushButton(self.groupBox_6) + self.pushButton_start_2.setObjectName("pushButton_start_2") + self.horizontalLayout_27.addWidget(self.pushButton_start_2) + self.pushButton_stop_2 = QtWidgets.QPushButton(self.groupBox_6) + self.pushButton_stop_2.setObjectName("pushButton_stop_2") + self.horizontalLayout_27.addWidget(self.pushButton_stop_2) + self.verticalLayout_7.addLayout(self.horizontalLayout_27) + self.verticalLayout_6.addWidget(self.groupBox_6) + self.groupBox_5 = QtWidgets.QGroupBox(self.tab_4) + self.groupBox_5.setObjectName("groupBox_5") + self.horizontalLayout_12 = QtWidgets.QHBoxLayout(self.groupBox_5) + self.horizontalLayout_12.setObjectName("horizontalLayout_12") + self.gridLayout_6 = QtWidgets.QGridLayout() + self.gridLayout_6.setObjectName("gridLayout_6") + self.label_28 = QtWidgets.QLabel(self.groupBox_5) + self.label_28.setAlignment(QtCore.Qt.AlignCenter) + self.label_28.setObjectName("label_28") + self.gridLayout_6.addWidget(self.label_28, 2, 0, 1, 1) + self.label_magn_y = QtWidgets.QLabel(self.groupBox_5) + self.label_magn_y.setAlignment(QtCore.Qt.AlignCenter) + self.label_magn_y.setObjectName("label_magn_y") + self.gridLayout_6.addWidget(self.label_magn_y, 2, 2, 1, 1) + self.label_26 = QtWidgets.QLabel(self.groupBox_5) + self.label_26.setAlignment(QtCore.Qt.AlignCenter) + self.label_26.setObjectName("label_26") + self.gridLayout_6.addWidget(self.label_26, 1, 0, 1, 1) + self.label_24 = QtWidgets.QLabel(self.groupBox_5) + self.label_24.setAlignment(QtCore.Qt.AlignCenter) + self.label_24.setObjectName("label_24") + self.gridLayout_6.addWidget(self.label_24, 0, 0, 1, 1) + self.label_acc_z = QtWidgets.QLabel(self.groupBox_5) + self.label_acc_z.setAlignment(QtCore.Qt.AlignCenter) + self.label_acc_z.setObjectName("label_acc_z") + self.gridLayout_6.addWidget(self.label_acc_z, 0, 3, 1, 1) + self.label_acc_y = QtWidgets.QLabel(self.groupBox_5) + self.label_acc_y.setAlignment(QtCore.Qt.AlignCenter) + self.label_acc_y.setObjectName("label_acc_y") + self.gridLayout_6.addWidget(self.label_acc_y, 0, 2, 1, 1) + self.label_gyro_x = QtWidgets.QLabel(self.groupBox_5) + self.label_gyro_x.setAlignment(QtCore.Qt.AlignCenter) + self.label_gyro_x.setObjectName("label_gyro_x") + self.gridLayout_6.addWidget(self.label_gyro_x, 1, 1, 1, 1) + self.label_gyro_z = QtWidgets.QLabel(self.groupBox_5) + self.label_gyro_z.setAlignment(QtCore.Qt.AlignCenter) + self.label_gyro_z.setObjectName("label_gyro_z") + self.gridLayout_6.addWidget(self.label_gyro_z, 1, 3, 1, 1) + self.label_gyro_y = QtWidgets.QLabel(self.groupBox_5) + self.label_gyro_y.setAlignment(QtCore.Qt.AlignCenter) + self.label_gyro_y.setObjectName("label_gyro_y") + self.gridLayout_6.addWidget(self.label_gyro_y, 1, 2, 1, 1) + self.label_magn_z = QtWidgets.QLabel(self.groupBox_5) + self.label_magn_z.setAlignment(QtCore.Qt.AlignCenter) + self.label_magn_z.setObjectName("label_magn_z") + self.gridLayout_6.addWidget(self.label_magn_z, 2, 3, 1, 1) + self.label_acc_x = QtWidgets.QLabel(self.groupBox_5) + self.label_acc_x.setAlignment(QtCore.Qt.AlignCenter) + self.label_acc_x.setObjectName("label_acc_x") + self.gridLayout_6.addWidget(self.label_acc_x, 0, 1, 1, 1) + self.label_magn_x = QtWidgets.QLabel(self.groupBox_5) + self.label_magn_x.setAlignment(QtCore.Qt.AlignCenter) + self.label_magn_x.setObjectName("label_magn_x") + self.gridLayout_6.addWidget(self.label_magn_x, 2, 1, 1, 1) + self.horizontalLayout_12.addLayout(self.gridLayout_6) + spacerItem14 = QtWidgets.QSpacerItem(615, 20, QtWidgets.QSizePolicy.Expanding, QtWidgets.QSizePolicy.Minimum) + self.horizontalLayout_12.addItem(spacerItem14) + self.verticalLayout_6.addWidget(self.groupBox_5) + self.tabWidget.addTab(self.tab_4, "") + self.gridLayout_2.addWidget(self.tabWidget, 1, 0, 1, 1) + self.gridLayout_2.setRowStretch(0, 1) + + self.retranslateUi(pb) + self.tabWidget.setCurrentIndex(1) + self.slider_wheel_diameter.valueChanged['int'].connect(self.label_wheel_diameter.setNum) # type: ignore + self.slider_wheel_track.valueChanged['int'].connect(self.label_wheel_track.setNum) # type: ignore + self.slider_encoder.valueChanged['int'].connect(self.label_encoder_res.setNum) # type: ignore + self.slider_motor_ratio.valueChanged['int'].connect(self.label_motor_ratio.setNum) # type: ignore + self.slider_pid_interval.valueChanged['int'].connect(self.label_pid_interval.setNum) # type: ignore + self.slider_kd.valueChanged['int'].connect(self.label_kd.setNum) # type: ignore + self.slider_kp.valueChanged['int'].connect(self.label_kp.setNum) # type: ignore + self.slider_ko.valueChanged['int'].connect(self.label_ko.setNum) # type: ignore + self.slider_ki.valueChanged['int'].connect(self.label_ki.setNum) # type: ignore + self.slider_cmd_lasttime.valueChanged['int'].connect(self.label_cmd_lasttime.setNum) # type: ignore + self.slider_va_max.valueChanged['int'].connect(self.label_va_max.setNum) # type: ignore + self.slider_vy_max.valueChanged['int'].connect(self.label_vy_max.setNum) # type: ignore + self.slider_vx_max.valueChanged['int'].connect(self.label_vx_max.setNum) # type: ignore + self.slider_set_pwm1.valueChanged['int'].connect(self.label_set_pwm1.setNum) # type: ignore + self.slider_set_pwm2.valueChanged['int'].connect(self.label_set_pwm2.setNum) # type: ignore + self.slider_set_pwm3.valueChanged['int'].connect(self.label_set_pwm3.setNum) # type: ignore + self.slider_set_pwm4.valueChanged['int'].connect(self.label_set_pwm4.setNum) # type: ignore + QtCore.QMetaObject.connectSlotsByName(pb) + + def retranslateUi(self, pb): + _translate = QtCore.QCoreApplication.translate + pb.setWindowTitle(_translate("pb", "PIBOT Test Tool")) + self.label_2.setText(_translate("pb", "Model Name")) + self.label_3.setText(_translate("pb", "IMU Type")) + self.label_6.setText(_translate("pb", "MotorRatio")) + self.label_motor_ratio.setText(_translate("pb", "0")) + self.label.setText(_translate("pb", "Diameter")) + self.label_wheel_diameter.setText(_translate("pb", "0")) + self.label_4.setText(_translate("pb", "Encoder")) + self.label_encoder_res.setText(_translate("pb", "0")) + self.label_5.setText(_translate("pb", "WheelTrack")) + self.label_wheel_track.setText(_translate("pb", "0")) + self.groupBox.setTitle(_translate("pb", "Reverse Direction Flag")) + self.checkBox_motor1.setText(_translate("pb", "Motor1 ")) + self.checkBox_motor2.setText(_translate("pb", "Motor2")) + self.checkBox_motor3.setText(_translate("pb", "Motor3")) + self.checkBox_motor4.setText(_translate("pb", "Motor4")) + self.checkBox_encoder1.setText(_translate("pb", "Encoder1")) + self.checkBox_encoder2.setText(_translate("pb", "Encoder2")) + self.checkBox_encoder3.setText(_translate("pb", "Encoder3")) + self.checkBox_encoder4.setText(_translate("pb", "Encoder4")) + self.groupBox_3.setTitle(_translate("pb", "PID")) + self.label_16.setText(_translate("pb", "KI")) + self.label_ki.setText(_translate("pb", "0")) + self.label_17.setText(_translate("pb", "KD")) + self.label_kd.setText(_translate("pb", "0")) + self.label_15.setText(_translate("pb", "KP")) + self.label_kp.setText(_translate("pb", "0")) + self.label_18.setText(_translate("pb", "KO")) + self.label_ko.setText(_translate("pb", "0")) + self.label_19.setText(_translate("pb", "Interval")) + self.label_pid_interval.setText(_translate("pb", "0")) + self.groupBox_2.setTitle(_translate("pb", "Speed Limit")) + self.label_22.setText(_translate("pb", "VY")) + self.label_vy_max.setText(_translate("pb", "0")) + self.label_21.setText(_translate("pb", "VX")) + self.label_vx_max.setText(_translate("pb", "0")) + self.label_23.setText(_translate("pb", "VA")) + self.label_va_max.setText(_translate("pb", "0")) + self.label_20.setText(_translate("pb", "CMD Time")) + self.label_cmd_lasttime.setText(_translate("pb", "0")) + self.pushButton_load.setText(_translate("pb", "Load")) + self.pushButton_set.setText(_translate("pb", "Set")) + self.pushButton_read.setText(_translate("pb", "Read")) + self.tabWidget.setTabText(self.tabWidget.indexOf(self.tab_3), _translate("pb", "Tab 1")) + self.groupBox_4.setTitle(_translate("pb", "Motor")) + self.label_7.setText(_translate("pb", "Motor1")) + self.label_set_pwm1.setText(_translate("pb", "0")) + self.label_10.setText(_translate("pb", "Encoder Feedback")) + self.label_feedback1.setText(_translate("pb", "0")) + self.label_8.setText(_translate("pb", "Motor2")) + self.label_set_pwm2.setText(_translate("pb", "0")) + self.label_11.setText(_translate("pb", "Encoder Feedback")) + self.label_feedback2.setText(_translate("pb", "0")) + self.label_9.setText(_translate("pb", "Motor3")) + self.label_set_pwm3.setText(_translate("pb", "0")) + self.label_12.setText(_translate("pb", "Encoder Feedback")) + self.label_feedback3.setText(_translate("pb", "0")) + self.label_13.setText(_translate("pb", "Motor4")) + self.label_set_pwm4.setText(_translate("pb", "0")) + self.label_14.setText(_translate("pb", "Encoder Feedback")) + self.label_feedback4.setText(_translate("pb", "0")) + self.pushButton_start.setText(_translate("pb", "start")) + self.pushButton_stop.setText(_translate("pb", "stop")) + self.groupBox_6.setTitle(_translate("pb", "PID Debug")) + self.label_25.setText(_translate("pb", "Input")) + self.label_input_1.setText(_translate("pb", "0")) + self.label_input_2.setText(_translate("pb", "0")) + self.label_input_3.setText(_translate("pb", "0")) + self.label_input_4.setText(_translate("pb", "0")) + self.label_27.setText(_translate("pb", "Feedback")) + self.label_output_1.setText(_translate("pb", "0")) + self.label_output_2.setText(_translate("pb", "0")) + self.label_output_3.setText(_translate("pb", "0")) + self.label_output_4.setText(_translate("pb", "0")) + self.label_29.setText(_translate("pb", "VY")) + self.label_30.setText(_translate("pb", "VY")) + self.label_31.setText(_translate("pb", "VA")) + self.pushButton_start_2.setText(_translate("pb", "start")) + self.pushButton_stop_2.setText(_translate("pb", "stop")) + self.groupBox_5.setTitle(_translate("pb", "IMU")) + self.label_28.setText(_translate("pb", "MAGN")) + self.label_magn_y.setText(_translate("pb", "0.000")) + self.label_26.setText(_translate("pb", "GYRO")) + self.label_24.setText(_translate("pb", "ACC")) + self.label_acc_z.setText(_translate("pb", "0.000")) + self.label_acc_y.setText(_translate("pb", "0.000")) + self.label_gyro_x.setText(_translate("pb", "0.000")) + self.label_gyro_z.setText(_translate("pb", "0.000")) + self.label_gyro_y.setText(_translate("pb", "0.000")) + self.label_magn_z.setText(_translate("pb", "0.000")) + self.label_acc_x.setText(_translate("pb", "0.000")) + self.label_magn_x.setText(_translate("pb", "0.000")) + self.tabWidget.setTabText(self.tabWidget.indexOf(self.tab_4), _translate("pb", "Tab 2")) diff --git a/Hardware/UPbot-Tools/pb.ui b/Hardware/UPbot-Tools/pb.ui new file mode 100644 index 0000000..cbca814 --- /dev/null +++ b/Hardware/UPbot-Tools/pb.ui @@ -0,0 +1,1613 @@ + + + pb + + + + 0 + 0 + 866 + 542 + + + + UPBot 图形化配置工具 + + + + + + Qt::Horizontal + + + + + + + 1 + + + + Tab 1 + + + + + + + + 10 + + + + + Model Name + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + + + 10 + + + + + IMU Type + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + + + + + Qt::Horizontal + + + + + + + + + + + 10 + + + 10 + + + + + MotorRatio + + + + + + + Qt::Horizontal + + + + + + + 0 + + + + + + + + + 10 + + + 10 + + + + + Diameter + + + + + + + Qt::Horizontal + + + + + + + 0 + + + + + + + + + 10 + + + 10 + + + + + Encoder + + + + + + + Qt::Horizontal + + + + + + + 0 + + + + + + + + + 10 + + + 10 + + + + + WheelTrack + + + + + + + Qt::Horizontal + + + + + + + 0 + + + + + + + + + + + + + Qt::Horizontal + + + + + + + + + false + + + Reverse Direction Flag + + + false + + + + + + + + 5 + + + + + Motor1 + + + + + + + Motor2 + + + + + + + Motor3 + + + + + + + Motor4 + + + + + + + Encoder1 + + + + + + + Encoder2 + + + + + + + Encoder3 + + + + + + + Encoder4 + + + + + + + + + + + + + + + + + + PID + + + + + + 10 + + + + + KI + + + + + + + Qt::Horizontal + + + + + + + 0 + + + + + + + + + 10 + + + + + KD + + + + + + + Qt::Horizontal + + + + + + + 0 + + + + + + + + + 10 + + + + + KP + + + + + + + Qt::Horizontal + + + + + + + 0 + + + + + + + + + 10 + + + + + KO + + + + + + + Qt::Horizontal + + + + + + + 0 + + + + + + + + + 10 + + + + + Interval + + + + + + + Qt::Horizontal + + + + + + + 0 + + + + + + + + + + + + Speed Limit + + + + + + 10 + + + + + VY + + + + + + + Qt::Horizontal + + + + + + + 0 + + + + + + + + + 10 + + + + + VX + + + + + + + Qt::Horizontal + + + + + + + 0 + + + + + + + + + 10 + + + + + VA + + + + + + + Qt::Horizontal + + + + + + + 0 + + + + + + + + + 10 + + + + + CMD Time + + + + + + + Qt::Horizontal + + + + + + + 0 + + + + + + + + + + + + + + Qt::Horizontal + + + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + Load + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + Set + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + Read + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + + + + Tab 2 + + + + + + Motor + + + + + + + + Motor1 + + + Qt::AlignCenter + + + + + + + Qt::Horizontal + + + + + + + 0 + + + Qt::AlignCenter + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + Feedback + + + + + + + 0 + + + Qt::AlignCenter + + + + + + + + + + + Motor2 + + + Qt::AlignCenter + + + + + + + Qt::Horizontal + + + + + + + 0 + + + Qt::AlignCenter + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + Feedback + + + + + + + 0 + + + Qt::AlignCenter + + + + + + + + + + + Motor3 + + + Qt::AlignCenter + + + + + + + Qt::Horizontal + + + + + + + 0 + + + Qt::AlignCenter + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + Feedback + + + + + + + 0 + + + Qt::AlignCenter + + + + + + + + + + + Motor4 + + + Qt::AlignCenter + + + + + + + Qt::Horizontal + + + + + + + 0 + + + Qt::AlignCenter + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + Feedback + + + + + + + 0 + + + Qt::AlignCenter + + + + + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + start + + + + + + + stop + + + + + + + + + + + + PID Debug + + + + + + + + Input + + + Qt::AlignCenter + + + + + + + 0 + + + Qt::AlignCenter + + + + + + + 0 + + + Qt::AlignCenter + + + + + + + 0 + + + Qt::AlignCenter + + + + + + + 0 + + + Qt::AlignCenter + + + + + + + + + + + Feedback + + + Qt::AlignCenter + + + + + + + 0 + + + Qt::AlignCenter + + + + + + + 0 + + + Qt::AlignCenter + + + + + + + 0 + + + Qt::AlignCenter + + + + + + + 0 + + + Qt::AlignCenter + + + + + + + + + + + VY + + + Qt::AlignCenter + + + + + + + VY + + + Qt::AlignCenter + + + + + + + VA + + + Qt::AlignCenter + + + + + + + start + + + + + + + stop + + + + + + + + + + + + IMU + + + + + + + + MAGN + + + Qt::AlignCenter + + + + + + + 0.000 + + + Qt::AlignCenter + + + + + + + GYRO + + + Qt::AlignCenter + + + + + + + ACC + + + Qt::AlignCenter + + + + + + + 0.000 + + + Qt::AlignCenter + + + + + + + 0.000 + + + Qt::AlignCenter + + + + + + + 0.000 + + + Qt::AlignCenter + + + + + + + 0.000 + + + Qt::AlignCenter + + + + + + + 0.000 + + + Qt::AlignCenter + + + + + + + 0.000 + + + Qt::AlignCenter + + + + + + + 0.000 + + + Qt::AlignCenter + + + + + + + 0.000 + + + Qt::AlignCenter + + + + + + + + + Qt::Horizontal + + + + 615 + 20 + + + + + + + + + + + + + + + + + slider_wheel_diameter + valueChanged(int) + label_wheel_diameter + setNum(int) + + + 218 + 98 + + + 395 + 98 + + + + + slider_wheel_track + valueChanged(int) + label_wheel_track + setNum(int) + + + 651 + 98 + + + 820 + 98 + + + + + slider_encoder + valueChanged(int) + label_encoder_res + setNum(int) + + + 214 + 142 + + + 395 + 142 + + + + + slider_motor_ratio + valueChanged(int) + label_motor_ratio + setNum(int) + + + 651 + 142 + + + 820 + 142 + + + + + slider_pid_interval + valueChanged(int) + label_pid_interval + setNum(int) + + + 246 + 330 + + + 412 + 330 + + + + + slider_kd + valueChanged(int) + label_kd + setNum(int) + + + 230 + 418 + + + 412 + 418 + + + + + slider_kp + valueChanged(int) + label_kp + setNum(int) + + + 229 + 359 + + + 412 + 359 + + + + + slider_ko + valueChanged(int) + label_ko + setNum(int) + + + 230 + 447 + + + 412 + 447 + + + + + slider_ki + valueChanged(int) + label_ki + setNum(int) + + + 226 + 388 + + + 412 + 388 + + + + + slider_cmd_lasttime + valueChanged(int) + label_cmd_lasttime + setNum(int) + + + 681 + 334 + + + 838 + 334 + + + + + slider_va_max + valueChanged(int) + label_va_max + setNum(int) + + + 655 + 443 + + + 838 + 443 + + + + + slider_vy_max + valueChanged(int) + label_vy_max + setNum(int) + + + 655 + 407 + + + 838 + 407 + + + + + slider_vx_max + valueChanged(int) + label_vx_max + setNum(int) + + + 656 + 370 + + + 838 + 370 + + + + + slider_set_pwm1 + valueChanged(int) + label_set_pwm1 + setNum(int) + + + 313 + 142 + + + 525 + 142 + + + + + slider_set_pwm2 + valueChanged(int) + label_set_pwm2 + setNum(int) + + + 322 + 165 + + + 528 + 165 + + + + + slider_set_pwm3 + valueChanged(int) + label_set_pwm3 + setNum(int) + + + 322 + 199 + + + 528 + 199 + + + + + slider_set_pwm4 + valueChanged(int) + label_set_pwm4 + setNum(int) + + + 322 + 232 + + + 528 + 232 + + + + + diff --git a/Hardware/UPbot-Tools/pypibot/__init__.py b/Hardware/UPbot-Tools/pypibot/__init__.py new file mode 100644 index 0000000..aa21836 --- /dev/null +++ b/Hardware/UPbot-Tools/pypibot/__init__.py @@ -0,0 +1,17 @@ +#!/usr/bin/python +# -*- coding: utf-8 -*- +import pypibot.assistant +import pypibot.log as Logger +import pypibot.err +log=Logger.log +import sys +def createNamedLog(name): + return Logger.NamedLog(name) +class Object(): + pass +isDebug="-d" in sys.argv + +import platform +isWindows=False +if platform.system()=='Windows': + isWindows=True diff --git a/Hardware/UPbot-Tools/pypibot/__pycache__/__init__.cpython-37.pyc b/Hardware/UPbot-Tools/pypibot/__pycache__/__init__.cpython-37.pyc new file mode 100644 index 0000000..e76284f Binary files /dev/null and b/Hardware/UPbot-Tools/pypibot/__pycache__/__init__.cpython-37.pyc differ diff --git a/Hardware/UPbot-Tools/pypibot/__pycache__/assistant.cpython-37.pyc b/Hardware/UPbot-Tools/pypibot/__pycache__/assistant.cpython-37.pyc new file mode 100644 index 0000000..c2b3710 Binary files /dev/null and b/Hardware/UPbot-Tools/pypibot/__pycache__/assistant.cpython-37.pyc differ diff --git a/Hardware/UPbot-Tools/pypibot/__pycache__/err.cpython-37.pyc b/Hardware/UPbot-Tools/pypibot/__pycache__/err.cpython-37.pyc new file mode 100644 index 0000000..5eff6b8 Binary files /dev/null and b/Hardware/UPbot-Tools/pypibot/__pycache__/err.cpython-37.pyc differ diff --git a/Hardware/UPbot-Tools/pypibot/__pycache__/log.cpython-37.pyc b/Hardware/UPbot-Tools/pypibot/__pycache__/log.cpython-37.pyc new file mode 100644 index 0000000..05b8abd Binary files /dev/null and b/Hardware/UPbot-Tools/pypibot/__pycache__/log.cpython-37.pyc differ diff --git a/Hardware/UPbot-Tools/pypibot/assistant.py b/Hardware/UPbot-Tools/pypibot/assistant.py new file mode 100644 index 0000000..94683b0 --- /dev/null +++ b/Hardware/UPbot-Tools/pypibot/assistant.py @@ -0,0 +1,234 @@ +#!/usr/bin/python +# -*- coding: utf-8 -*- +import os, sys, inspect +import datetime +import signal +import threading +import time +# function: get directory of current script, if script is built +# into an executable file, get directory of the excutable file +def current_file_directory(): + path = os.path.realpath(sys.path[0]) # interpreter starter's path + if os.path.isfile(path): # starter is excutable file + path = os.path.dirname(path) + path = os.path.abspath(path) # return excutable file's directory + else: # starter is python script + caller_file = inspect.stack()[0][1] # function caller's filename + path = os.path.abspath(os.path.dirname(caller_file))# return function caller's file's directory + if path[-1]!=os.path.sep:path+=os.path.sep + return path + +"""格式化字符串""" +def formatString(string,*argv): + string=string%argv + if string.find('$(scriptpath)')>=0: + string=string.replace('$(scriptpath)',current_file_directory()) + if string.find('$(filenumber2)')>=0: + i=0 + path="" + while True: + path=string.replace('$(scriptpath)',"%02d"%i) + if not os.path.lexists(path):break + i+=1 + string=path + #8位日期（20140404） + if string.find('$(Date8)')>=0: + now=datetime.datetime.now() + string=string.replace('$(Date8)', now.strftime("%Y%m%d")) + #6位日期（140404） + if string.find('$(Date6)')>=0: + now=datetime.datetime.now() + string=string.replace('$(Date6)', now.strftime("%y%m%d")) + #6位时间（121212） + if string.find('$(Time6)')>=0: + now=datetime.datetime.now() + string=string.replace('$(Time6)', now.strftime("%H%M%S")) + #4位时间（1212） + if string.find('$(Time4)')>=0: + now=datetime.datetime.now() + string=string.replace('$(Time4)', now.strftime("%H%M")) + #文件编号2位（必须在最后） + if string.find('$(filenumber2)')>=0: + i=0 + path="" + while True: + path=string.replace('$(filenumber2)',"%02d"%i) + if not os.path.lexists(path):break + i+=1 + string=path + #文件编号3位（必须在最后） + if string.find('$(filenumber3)')>=0: + i=0 + path="" + while True: + path=string.replace('$(filenumber3)',"%03d"%i) + if not os.path.lexists(path):break + i+=1 + string=path + return string + +""" + 取得进程列表 + 格式：(PID,cmd) +""" +def getProcessList(): + processList = [] + try: + for line in os.popen("ps xa"): + fields = line.split() + # spid = fields[0] + pid = 0 + try:pid = int(fields[0]) + except:None + cmd = line[27:-1] + # print "PS:%d,%s"%(npid,process) + if pid != 0 and len(cmd) > 0: + processList.append((pid, cmd)) + except Exception as e: + print("getProcessList except:%s" % (e)) + return processList +def killCommand(cmd): + try: + processList = getProcessList() + for p in processList: + if p[1].find(cmd) != -1: + pid = p[0] + os.kill(pid, signal.SIGKILL) + except Exception as e: + print("killCommand ‘%s’ except:%s" % (cmd,e)) + +def check_pid(pid): + """ Check For the existence of a unix pid. """ + if pid == 0:return False + try: + os.kill(pid, 0) + except OSError: + return False + else: + return True + +SF=formatString + +#全局异常捕获 +def excepthook(excType, excValue, tb): + '''''write the unhandle exception to log''' + from log import log + import traceback + log.e('Unhandled Error: %s',''.join(traceback.format_exception(excType, excValue, tb))) + sys.exit(-1) + #sys.__excepthook__(type, value, trace) + #sys.__excepthook__(excType, excValue, tb) + +_defaultGlobalExcept=sys.excepthook + +def enableGlobalExcept(enable=True): + if enable: + sys.excepthook = excepthook + else: + sys.excepthook=_defaultGlobalExcept +# 默认启动全局异常处理 +enableGlobalExcept() +#创建线程 +def createThread(name,target,args=(),autoRun=True,daemon=True): + from log import log + def threadProc(): + log.i("thread %s started!",name) + try: + target(*args) + log.i("thread %s ended!",name) + except Exception as e: + log.e("thread %s crash!err=%s",name,e) + thd=threading.Thread(name=name,target=threadProc) + thd.setDaemon(daemon) + if autoRun:thd.start() + return thd + + +#定时器 +class Timer(): + def __init__(self, timer_proc, args=(),first=0,period=0,name="Timer"): + self.name=name + self.first=first + self.period=period + self.args=args + self.timer_proc=timer_proc + self.thdWork=None + self.stopFlag=False + from log import NamedLog + self.log=NamedLog(name) + def run(self): + if self.first>0: + time.sleep(self.first) + try: + self.timer_proc(*self.args) + except Exception as e: + self.log.error("timer proc crash!err=%s",e) + while self.period>0 and not self.stopFlag: + time.sleep(self.period) + try: + self.timer_proc(*self.args) + except Exception as e: + self.log.error("timer proc crash!err=%s",e) + def start(self): + if self.isAlive(): + self.log.d("already running!") + return True + self.stopFlag=False + self.thdWork=threading.Thread(name=self.name,target=self.run) + self.thdWork.setDaemon(True) + self.thdWork.start() + def stop(self,timeout=3): + if self.isAlive(): + self.stopFlag=True + try: + self.thdWork.join(timeout) + except Exception as e: + self.log.e("stop timeout!") + + def isAlive(self): + return self.thdWork and self.thdWork.isAlive() +#计时器 +class Ticker(): + def __init__(self): + self.reset() + # 片段,可以判断时长是否在一个特定毫秒段内 + self.section=[] + def reset(self): + self.tick=time.time() + self.end=0 + def stop(self): + self.end=time.time() + @property + def ticker(self): + if self.end==0: + return int((time.time()-self.tick)*1000) + else: + return int((self.end-self.tick)*1000) + def addSection(self,a,b): + a,b=int(a),int(b) + assert a=a and tick<=b: + return True + return False + def __call__(self): + return self.ticker +def waitExit(): + import log + log.log.i("start waiting to exit...") + try: + while(True): + time.sleep(1) + except Exception as e: + log.log.w("recv exit sign!") + +def is_python3(): + return sys.hexversion > 0x03000000 diff --git a/Hardware/UPbot-Tools/pypibot/configer.py b/Hardware/UPbot-Tools/pypibot/configer.py new file mode 100644 index 0000000..7bb3b9f --- /dev/null +++ b/Hardware/UPbot-Tools/pypibot/configer.py @@ -0,0 +1,56 @@ +#!/usr/bin/python +# -*- coding: utf-8 -*- + +import ConfigParser +from log import PLOG +import os +def getdefaultfilename(): + pass +def openconfiger(filename): + return configer(filename) +class configer: + def __init__(self,fullfilepath): + self._filepath=fullfilepath + if not os.path.isdir(os.path.dirname(fullfilepath)): + os.makedirs(os.path.dirname(fullfilepath)) + self._conf=ConfigParser.ConfigParser() + if os.path.isfile(fullfilepath): + try: + self._conf.readfp(open(fullfilepath,"r")) + except Exception,e: + PLOG.error("配置文件'%s'打开失败,err=%s"%(self._filepath,e)) + def save(self): + try: + self._conf.write(open(self._filepath,"w")) + except Exception,e: + PLOG.error("配置文件'%s'保存失败,err=%s"%(self._filepath,e)) + + def changeConfValue(self,section,option,value): + if self._conf.has_section(section): + self._conf.set(section,option,value) + else: + self._conf.add_section(section) + self._conf.set(section,option,value) + + def _readvalue(self,fn,section,option,default): + result=default + if self._conf.has_section(section): + if self._conf.has_option(section,option): + result=fn(section,option) + PLOG.debug("Option[%s][%s]=%s"%(section,option,str(result))) + else: + self._conf.set(section,option,str(default)) + result=default + else: + self._conf.add_section(section) + self._conf.set(section,option,str(default)) + result=default + return result + def getstr(self,section,option,default=""): + return self._readvalue(self._conf.get, section, option, default) + def getint(self,section,option,default=0): + return self._readvalue(self._conf.getint, section, option, default) + def getfloat(self,section,option,default=0.0): + return self._readvalue(self._conf.getfloat, section, option, default) + def getboolean(self,section,option,default=False): + return self._readvalue(self._conf.getboolean, section, option, default) diff --git a/Hardware/UPbot-Tools/pypibot/daemon.py b/Hardware/UPbot-Tools/pypibot/daemon.py new file mode 100644 index 0000000..1ff78dc --- /dev/null +++ b/Hardware/UPbot-Tools/pypibot/daemon.py @@ -0,0 +1,140 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +import sys, os, time, atexit +from signal import SIGTERM + +def check_pid(pid): + """ Check For the existence of a unix pid. """ + if pid == 0:return False + try: + os.kill(pid, 0) + except OSError: + return False + else: + return True + +class daemon: + """ + A generic daemon class. + + Usage: subclass the Daemon class and override the run() method + """ + def __init__(self, pidfile, stdin='/dev/null', stdout='/dev/null', stderr='/dev/null'): + self.stdin = stdin + self.stdout = stdout + self.stderr = stderr + self.pidfile = pidfile + + def daemonize(self): + """ + do the UNIX double-fork magic, see Stevens' "Advanced + Programming in the UNIX Environment" for details (ISBN 0201563177) + http://www.erlenstar.demon.co.uk/unix/faq_2.html#SEC16 + """ + try: + pid = os.fork() + if pid > 0: + # exit first parent + sys.exit(0) + except OSError, e: + sys.stderr.write("fork #1 failed: %d (%s)\n" % (e.errno, e.strerror)) + sys.exit(1) + + # decouple from parent environment + os.chdir("/") + os.setsid() + os.umask(0) + + # do second fork + try: + pid = os.fork() + if pid > 0: + # exit from second parent + sys.exit(0) + except OSError, e: + sys.stderr.write("fork #2 failed: %d (%s)\n" % (e.errno, e.strerror)) + sys.exit(1) + + # redirect standard file descriptors + sys.stdout.flush() + sys.stderr.flush() + si = file(self.stdin, 'r') + so = file(self.stdout, 'a+') + se = file(self.stderr, 'a+', 0) + os.dup2(si.fileno(), sys.stdin.fileno()) + os.dup2(so.fileno(), sys.stdout.fileno()) + os.dup2(se.fileno(), sys.stderr.fileno()) + + # write pidfile + atexit.register(self.delpid) + pid = str(os.getpid()) + file(self.pidfile, 'w+').write("%s\n" % pid) + + def delpid(self): + os.remove(self.pidfile) + + def start(self): + """ + Start the daemon + """ + # Check for a pidfile to see if the daemon already runs + try: + pf = file(self.pidfile, 'r') + pid = int(pf.read().strip()) + pf.close() + except IOError: + pid = None + + if pid and check_pid(pid): + message = "pidfile %s already exist. Daemon already running?\n" + sys.stderr.write(message % self.pidfile) + sys.exit(1) + print("daemon start...") + # Start the daemon + self.daemonize() + self.run() + + def stop(self): + """ + Stop the daemon + """ + # Get the pid from the pidfile + try: + pf = file(self.pidfile, 'r') + pid = int(pf.read().strip()) + pf.close() + except IOError: + pid = None + + if not pid: + message = "pidfile %s does not exist. Daemon not running?\n" + sys.stderr.write(message % self.pidfile) + return # not an error in a restart + + # Try killing the daemon process + try: + while 1: + os.kill(pid, SIGTERM) + time.sleep(0.1) + except OSError, err: + err = str(err) + if err.find("No such process") > 0: + if os.path.exists(self.pidfile): + os.remove(self.pidfile) + else: + print(str(err)) + sys.exit(1) + + def restart(self): + """ + Restart the daemon + """ + self.stop() + self.start() + + def run(self): + """ + You should override this method when you subclass Daemon. It will be called after the process has been + daemonized by start() or restart(). + """ diff --git a/Hardware/UPbot-Tools/pypibot/err.py b/Hardware/UPbot-Tools/pypibot/err.py new file mode 100644 index 0000000..d812de9 --- /dev/null +++ b/Hardware/UPbot-Tools/pypibot/err.py @@ -0,0 +1,58 @@ +#!/usr/bin/python +# -*- coding: utf-8 -*- + +# 异常类 +class PibotError(Exception): + def __init__(self, errcode, errmsg): + self.errcode = errcode + self.errmsg = errmsg + #Exception.__init__(self,self.__str__()) + + def msg(self, msg): + if not msg is None:return PibotError(self.errcode, msg) + return PibotError(8,"unknow error") + def __str__(self): + return "PibotError:%s(%d)"%(self.errmsg,self.errcode) + @property + def message(self): + return str(self) +# 声明 +# 成功 +success=PibotError(0,"null") +# 通用失败 +fail=PibotError(1,"fail") +# 参数无效 +invalidParameter=PibotError(2,"invalid parameter") +# 不支持 +noSupport=PibotError(3,"no support") +# 不存在 +noExist=PibotError(4,"no exist") +# 超时 +timeout=PibotError(5,"timeout") +# 繁忙 +busy=PibotError(6,"busy") +# 缺少参数 +missParameter=PibotError(7,"miss parameter") +# 系统错误(通用错误) +systemError=PibotError(8,"system error") +# 密码错误 +invalidPassword=PibotError(9,"invalid password") +# 编码失败 +encodeFailed=PibotError(10,"encode failed") +# 数据库操作失败 +dbOpertationFailed=PibotError(11,"db error") +# 已占用 +occupied=PibotError(12,"occupied") +# session不存在 +noSession = PibotError(13,'cannot find session') +#没有找到 +noFound = PibotError(14, "no found") +#已经存在 +existed = PibotError(15, "existed") +#已经锁定 +locked = PibotError(16, "locked") +#已经过期 +expired = PibotError(17, "is expired") +#无效的参数 +invalidParameter = PibotError(18, "invalid parameter") + diff --git a/Hardware/UPbot-Tools/pypibot/log.py b/Hardware/UPbot-Tools/pypibot/log.py new file mode 100644 index 0000000..c031d4a --- /dev/null +++ b/Hardware/UPbot-Tools/pypibot/log.py @@ -0,0 +1,259 @@ +#!/usr/bin/python +# -*- coding: utf-8 -*- +import sys,os +import datetime +import threading +import pypibot.assistant as assistant +import platform +if assistant.is_python3(): + import _thread +else: + import thread +import traceback +""" +%a Locale’s abbreviated weekday name. +%A Locale’s full weekday name. +%b Locale’s abbreviated month name. +%B Locale’s full month name. +%c Locale’s appropriate date and time representation. +%d Day of the month as a decimal number [01,31]. +%H Hour (24-hour clock) as a decimal number [00,23]. +%I Hour (12-hour clock) as a decimal number [01,12]. +%j Day of the year as a decimal number [001,366]. +%m Month as a decimal number [01,12]. +%M Minute as a decimal number [00,59]. +%p Locale’s equivalent of either AM or PM. (1) +%S Second as a decimal number [00,61]. (2) +%U Week number of the year (Sunday as the first day of the week) as a decimal number [00,53]. All days in a new year preceding the first Sunday are considered to be in week 0. (3) +%w Weekday as a decimal number [0(Sunday),6]. +%W Week number of the year (Monday as the first day of the week) as a decimal number [00,53]. All days in a new year preceding the first Monday are considered to be in week 0. (3) +%x Locale’s appropriate date representation. +%X Locale’s appropriate time representation. +%y Year without century as a decimal number [00,99]. +%Y Year with century as a decimal number. +%Z Time zone name (no characters if no time zone exists). +%% A literal '%' character. + +""" +isWindows=False +if platform.system()=='Windows': + isWindows=True +defaultEncodeing="utf8" +if "-utf8" in sys.argv: + defaultEncodeing="utf-8" +if "-gbk" in sys.argv: + defaultEncodeing="gbk" + +TRACE=5 +DEBUG=4 +INFORMATION=3 +WARNING=2 +ERROR=1 +NONE=0 + +MAX_MSG_SIZE = 4096 + +def getLevelFromString(level): + level=level.lower() + if level=='t' or level=='trace':return 5 + elif level=='d' or level=='debug':return 4 + elif level=='i' or level=='info':return 3 + elif level=='w' or level=='wran':return 2 + elif level=='e' or level=='error':return 1 + else :return 0 +def getLevelString(level): + if level==TRACE:return "T" + elif level==DEBUG:return "D" + elif level==INFORMATION:return "I" + elif level==WARNING:return "W" + elif level==ERROR:return "E" + else:return "N" +class PibotLog: + def __init__(self): + self.isEnableControlLog=True + self.fileTemple=None + self.filePath="" + self.level=5 + self._lock=threading.RLock() + self.fd=None + self.fd_day=-1 + def setLevel(self,level): + self.level=getLevelFromString(level) + def enableControllog(self,enable): + self.isEnableControlLog=enable + def enableFileLog(self,fileName): + self.fileTemple=fileName + self.updateFilelog() + def updateFilelog(self): + fn=assistant.SF(self.fileTemple) + if fn!=self.filePath: + self.i("new log file:%s",fn) + if self.fd: + self.fd.close() + self.fd=None + self.fd_day=-1 + self.filePath="" + try: + path=os.path.dirname(fn) + if path!="" and not os.path.isdir(path):os.makedirs(path) + self.fd=open(fn,mode="w") + try: + linkfn = fn.split(".log.", 1)[0] + ".INFO" + if os.path.exists(linkfn): + os.remove(linkfn) + (filepath, tempfilename) = os.path.split(fn) + os.symlink(tempfilename, linkfn) + except: + pass + self.fd_day=datetime.datetime.now().day + self.filePath=fn + except Exception as e: + print("open file fail!file=%s,err=%s"%(fn,e)) + def _output(self,level,msg,args): + if self.level0: + t=[] + for arg in args: + if isinstance(arg,Exception): + t.append(traceback.format_exc(arg).decode('utf-8')) + elif isinstance(arg,bytes) : + t.append(arg.decode('utf-8')) + else: + t.append(arg) + args=tuple(t) + try: + msg=msg%args + except: + try: + for arg in args: + msg=msg+str(arg)+" " + except Exception as e: + msg=msg+"==LOG ERROR==>%s"%(e) + if len(msg)>MAX_MSG_SIZE:msg=msg[0:MAX_MSG_SIZE] + now=datetime.datetime.now() + msg=msg+"\r\n" + if assistant.is_python3(): + id = _thread.get_ident() + else: + id = thread.get_ident() + s="[%s] %04d-%02d-%02d %02d:%02d:%02d.%03d (0x%04X):%s"%(getLevelString(level),now.year,now.month,now.day,now.hour,now.minute,now.second,now.microsecond/1000,(id>>8)&0xffff,msg) + prefix='' + suffix='' + if not isWindows: + suffix='\033[0m' + if level==TRACE: + prefix='\033[0;37m' + elif level==DEBUG: + prefix='\033[1m' + elif level==INFORMATION: + prefix='\033[0;32m' + elif level==WARNING: + prefix='\033[0;33m' + elif level==ERROR: + prefix='\033[0;31m' + else: + pass + self._lock.acquire() + try: + if self.isEnableControlLog: + sys.stdout.write((prefix+s+suffix)) + if self.fd: + if self.fd_day!=now.day: + self.updateFilelog() + if assistant.is_python3(): + self.fd.write(s) + else: + self.fd.write(s.encode('utf-8')) + self.fd.flush() + finally: + self._lock.release() + except Exception as e: + if assistant.is_python3(): + print(e) + else: + print("PibotLog._output crash!err=%s"%traceback.format_exc(e)) + + def trace(self,msg,*args): + self._output(TRACE,msg,args) + def t(self,msg,*args): + self._output(TRACE,msg,args) + def debug(self,msg,*args): + self._output(DEBUG, msg,args) + def d(self,msg,*args): + self._output(DEBUG, msg,args) + def info(self,msg,*args): + self._output(INFORMATION, msg,args) + def i(self,msg,*args): + self._output(INFORMATION, msg,args) + def warn(self,msg,*args): + self._output(WARNING, msg,args) + def w(self,msg,*args): + self._output(WARNING, msg,args) + def error(self,msg,*args): + self._output(ERROR, msg,args) + def e(self,msg,*args): + self._output(ERROR, msg,args) + def _log(self,level,msg,args): + self._output(level, msg,args) + def createNamedLog(self,name): + return NamedLog(name) +log=PibotLog() +class NamedLog: + def __init__(self,name=None): + global log + self.name='' + if name: + self.name='['+name+']' + self.log=log + def trace(self,msg,*args): + msg=self.name+msg + self.log._log(TRACE,msg,args) + def t(self,msg,*args): + msg=self.name+msg + self.log._log(TRACE,msg,args) + def debug(self,msg,*args): + msg=self.name+msg + self.log._log(DEBUG, msg,args) + def d(self,msg,*args): + msg=self.name+msg + self.log._log(DEBUG, msg,args) + def info(self,msg,*args): + msg=self.name+msg + self.log._log(INFORMATION, msg,args) + def i(self,msg,*args): + msg=self.name+msg + self.log._log(INFORMATION, msg,args) + def warn(self,msg,*args): + msg=self.name+msg + self.log._log(WARNING, msg,args) + def w(self,msg,*args): + msg=self.name+msg + self.log._log(WARNING, msg,args) + def error(self,msg,*args): + msg=self.name+msg + self.log._log(ERROR, msg,args) + def e(self,msg,*args): + msg=self.name+msg + self.log._log(ERROR, msg,args) + +if __name__ == "__main__": + log.trace("1%s","hello") + log.debug("2%d",12) + try: + raise Exception("EXC") + except Exception as e: + log.info("3%s",e) + log.warn("1") + log.error("1") + #log.enableFileLog("$(scriptpath)test_$(Date8)_$(filenumber2).log") + log.trace("1") + log.debug("1") + log.info("1") + log.warn("1") + log.error("1") + log=NamedLog("test") + log.d("1") + log.i("1") + log.warn("1") + log.error("1=%d,%s",100,e) diff --git a/Hardware/UPbot-Tools/pypibot/mapconvert.py b/Hardware/UPbot-Tools/pypibot/mapconvert.py new file mode 100644 index 0000000..0d27ec4 --- /dev/null +++ b/Hardware/UPbot-Tools/pypibot/mapconvert.py @@ -0,0 +1,35 @@ +import cv2 +import numpy as np + +def map_server_to_cv2(data): + # print(data.info.height, data.info.width) + data = np.reshape(data.data, (data.info.height, data.info.width)) + data = np.flipud(data) + + # -1 --> 205 + # 0 --> 255 + # 100--> 0 + image = np.where(data == -1, 205, 255 - (255.0 * data / 100)).astype(np.uint8) + + #cv2.imshow("cv2_map", data) + return image + + +def cv2_to_map_server(image): + image = np.flipud(image) + + # 205 --> -1 + # 255 --> 0 + # 0 --> 100 + data = np.where(image == 205, -1, (255 - image) * 100.0 / 255).astype(np.int8).flatten() + + return list(data) + # pub_data = OccupancyGrid() + # pub_data.header.frame_id = "map" + # pub_data.header.stamp = rospy.Time.now() + # pub_data.info = map_data.info + # pub_data.data = list(data) + + # print(len(pub_data.data), pub_data.info.height * pub_data.info.width) + + # map_publisher.publish(pub_data) \ No newline at end of file diff --git a/Hardware/UPbot-Tools/pypibot/proxy.py b/Hardware/UPbot-Tools/pypibot/proxy.py new file mode 100644 index 0000000..5187c22 --- /dev/null +++ b/Hardware/UPbot-Tools/pypibot/proxy.py @@ -0,0 +1,38 @@ +from pypibot import log + +import threading +import zmq + +class MqProxy: + def __init__(self, sub_addr, pub_addr): + self.thd = None + self.sub_addr = sub_addr + self.pub_addr = pub_addr + + def _run(self): + context = zmq.Context() + + frontend = context.socket(zmq.XSUB) + frontend.bind(self.sub_addr) + # frontend.bind("tcp://*:5556") + backend = context.socket(zmq.XPUB) + backend.bind(self.pub_addr) + # backend.bind("tcp://*:5557") + try: + zmq.proxy(frontend, backend) + except KeyboardInterrupt: + pass + + frontend.close() + backend.close() + context.term() + + def start(self): + if self.thd is None: + log.i("mq proxy starting...") + self.thd=threading.Thread(target=self._run, name="proxy") + self.thd.setDaemon(True) + self.thd.start() + + def stop(self): + pass diff --git a/Hardware/UPbot-Tools/set_default_params.py b/Hardware/UPbot-Tools/set_default_params.py new file mode 100644 index 0000000..096b17c --- /dev/null +++ b/Hardware/UPbot-Tools/set_default_params.py @@ -0,0 +1,90 @@ +import platform +import sys +sys.path.append("..") +import pypibot +from pypibot import log +from transport import Transport +from dataholder import MessageID +import params + +#for linux +port="/dev/pibot" + +#for windows +#port="com3" + +pypibot.assistant.enableGlobalExcept() +#log.enableFileLog(log_dir + "ros_$(Date8)_$(filenumber2).log") +log.setLevel("i") + +if __name__ == '__main__': + mboard = Transport(port, params.pibotBaud) + if not mboard.start(): + log.error("can not open %s"%port) + sys.exit() + + DataHolder = mboard.getDataHolder() + + for num in range(0,3): + log.info("****************get robot version*****************") + boardVersion = DataHolder[MessageID.ID_GET_VERSION] + p = mboard.request(MessageID.ID_GET_VERSION) + if p: + log.info("firmware version:%s buildtime:%s\r\n"%(boardVersion.version.decode(), boardVersion.build_time.decode())) + break + else: + log.error('read firmware version err\r\n') + import time + time.sleep(1) + if num == 2: + log.error('please check connection or baudrate\r\n') + sys.exit() + + # set robot parameter + log.info("****************set robot parameter*****************") + + DataHolder[MessageID.ID_SET_ROBOT_PARAMETER].param = params.pibotParam + + p = mboard.request(MessageID.ID_SET_ROBOT_PARAMETER) + if p: + log.info('set parameter success') + else: + log.error('set parameter err') + quit(1) + + # get robot parameter + robotParam = DataHolder[MessageID.ID_GET_ROBOT_PARAMETER] + p = 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\r\n" \ + %(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\r\n" \ + %(robotParam.param.motor_nonexchange_flag,\ + robotParam.param.encoder_nonexchange_flag + )) + else: + log.error('get param err\r\n') + quit(1) \ No newline at end of file diff --git a/Hardware/UPbot-Tools/test_motors.py b/Hardware/UPbot-Tools/test_motors.py new file mode 100644 index 0000000..d5e0daa --- /dev/null +++ b/Hardware/UPbot-Tools/test_motors.py @@ -0,0 +1,122 @@ +import platform +import sys +sys.path.append("..") +import pypibot +from pypibot import log +from transport import Transport +from dataholder import MessageID +import params +import signal + +#for linux +port="/dev/pibot" + +#for windows +#port="com3" + +pypibot.assistant.enableGlobalExcept() +#log.enableFileLog(log_dir + "ros_$(Date8)_$(filenumber2).log") +log.setLevel("i") + +run_flag = True + +def exit(signum, frame): + global run_flag + run_flag = False + +if __name__ == '__main__': + signal.signal(signal.SIGINT, exit) + + mboard = Transport(port, params.pibotBaud) + if not mboard.start(): + log.error("can not open %s"%port) + sys.exit() + + pwm=[0]*4 + for i in range(len(sys.argv)-1): + pwm[i] = int(sys.argv[i+1]) + + DataHolder = mboard.getDataHolder() + + for num in range(0,3): + log.info("****************get robot version*****************") + boardVersion = DataHolder[MessageID.ID_GET_VERSION] + p = mboard.request(MessageID.ID_GET_VERSION) + if p: + log.info("firmware version:%s buildtime:%s\r\n"%(boardVersion.version.decode(), boardVersion.build_time.decode())) + break + else: + log.error('read firmware version err\r\n') + import time + time.sleep(1) + if num == 2: + log.error('please check connection or baudrate\r\n') + sys.exit() + + # get robot parameter + robotParam = DataHolder[MessageID.ID_GET_ROBOT_PARAMETER] + p = 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\r\n" \ + %(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\r\n" \ + %(robotParam.param.motor_nonexchange_flag,\ + robotParam.param.encoder_nonexchange_flag + )) + else: + log.error('get params err\r\n') + quit(1) + + DataHolder[MessageID.ID_SET_MOTOR_PWM].pwm = pwm + + p = mboard.request(MessageID.ID_SET_MOTOR_PWM) + if p: + log.info('set pwm success') + else: + log.error('set pwm err') + quit(1) + + log.info("****************get encoder count*****************") + while run_flag: + robotEncoder = DataHolder[MessageID.ID_GET_ENCODER_COUNT].encoder + p = mboard.request(MessageID.ID_GET_ENCODER_COUNT) + if p: + log.info('encoder count: %s'%(('\t\t').join([str(x) for x in robotEncoder]))) + else: + log.error('get encoder count err') + quit(1) + + import time + time.sleep(0.5) + + DataHolder[MessageID.ID_SET_MOTOR_PWM].pwm = [0]*4 + + p = mboard.request(MessageID.ID_SET_MOTOR_PWM) + if p: + log.info('set pwm success') + else: + log.error('set pwm err') + quit(1) \ No newline at end of file diff --git a/Hardware/UPbot-Tools/transport.py b/Hardware/UPbot-Tools/transport.py new file mode 100644 index 0000000..d3c63db --- /dev/null +++ b/Hardware/UPbot-Tools/transport.py @@ -0,0 +1,203 @@ +import sys +sys.path.append("..") +import pypibot +from pypibot import log +from pypibot import assistant + +import serial +import threading +import struct +import time +from dataholder import MessageID, BoardDataDict +FIX_HEAD = 0x5a + +class Recstate(): + WAITING_HD = 0 + WAITING_MSG_ID = 1 + RECEIVE_LEN = 2 + RECEIVE_PACKAGE = 3 + RECEIVE_CHECK = 4 + +def checksum(d): + sum = 0 + if assistant.is_python3(): + for i in d: + sum += i + sum = sum&0xff + else: + for i in d: + sum += ord(i) + sum = sum&0xff + return sum + + +class Transport: + def __init__(self, port, baudrate=921600): + self._Port = port + self._Baudrate = baudrate + self._KeepRunning = False + self.receive_state = Recstate.WAITING_HD + self.rev_msg = [] + self.rev_data = [] + self.req_lock = threading.Lock() + self.wait_event = threading.Event() + + def getDataHolder(self): + return BoardDataDict + + def start(self): + try: + self._Serial = serial.Serial(port=self._Port, baudrate=self._Baudrate, timeout=0.2) + self._KeepRunning = True + self._ReceiverThread = threading.Thread(target=self._Listen) + self._ReceiverThread.setDaemon(True) + self._ReceiverThread.start() + return True + except: + print("Open Serial Error") + return False + + def Stop(self): + self._KeepRunning = False + time.sleep(0.1) + self._Serial.close() + + def _Listen(self): + while self._KeepRunning: + if self.receiveFiniteStates(self._Serial.read()): + self.packageAnalysis() + + def receiveFiniteStates(self, s): + if len(s) == 0: + return False + val = s[0] + val_int = val + if not assistant.is_python3(): + val_int = ord(val) + + if self.receive_state == Recstate.WAITING_HD: + if val_int == FIX_HEAD: + log.trace('got head') + self.rev_msg = [] + self.rev_data =[] + self.rev_msg.append(val) + self.receive_state = Recstate.WAITING_MSG_ID + elif self.receive_state == Recstate.WAITING_MSG_ID: + log.trace('got msg id') + self.rev_msg.append(val) + self.receive_state = Recstate.RECEIVE_LEN + elif self.receive_state == Recstate.RECEIVE_LEN: + log.trace('got len:%d', val_int) + self.rev_msg.append(val) + if val_int == 0: + self.receive_state = Recstate.RECEIVE_CHECK + else: + self.receive_state = Recstate.RECEIVE_PACKAGE + elif self.receive_state == Recstate.RECEIVE_PACKAGE: + # if assistant.is_python3(): + # self.rev_data.append((chr(val)).encode('latin1')) + # else: + self.rev_data.append(val) + r = False + if assistant.is_python3(): + r = len(self.rev_data) == int(self.rev_msg[-1]) + else: + r = len(self.rev_data) == ord(self.rev_msg[-1]) + + if r: + self.rev_msg.extend(self.rev_data) + self.receive_state = Recstate.RECEIVE_CHECK + elif self.receive_state == Recstate.RECEIVE_CHECK: + log.trace('got check') + self.receive_state = Recstate.WAITING_HD + if val_int == checksum(self.rev_msg): + log.trace('got a complete message') + return True + else: + self.receive_state = Recstate.WAITING_HD + + # continue receiving + return False + + def packageAnalysis(self): + if assistant.is_python3(): + in_msg_id = int(self.rev_msg[1]) + else: + in_msg_id = ord(self.rev_msg[1]) + if assistant.is_python3(): + log.debug("recv body:" + " ".join("{:02x}".format(c) for c in self.rev_data)) + r = BoardDataDict[in_msg_id].unpack(bytes(self.rev_data)) + else: + log.debug("recv body:" + " ".join("{:02x}".format(ord(c)) for c in self.rev_data)) + r = BoardDataDict[in_msg_id].unpack(''.join(self.rev_data)) + if r: + self.res_id = in_msg_id + if in_msg_id<100: + self.set_response() + else:#notify + log.debug('msg %d'%self.rev_msg[4],'data incoming') + pass + else: + log.debug ('error unpacking pkg') + + def request(self, id, timeout=0.5): + self.req_lock.acquire() + if not self.write(id): + log.debug ('Serial send error!') + self.req_lock.release() + return False + if self.wait_for_response(timeout): + if id == self.res_id: + log.trace ('OK') + else: + log.error ('Got unmatched response!') + else: + log.error ('Request got no response!') + self.req_lock.release() + return False + # clear response + self.req_lock.release() + self.res_id = None + return True + + def write(self, id): + cmd = self.make_command(id) + if assistant.is_python3(): + log.d("write:" + " ".join("{:02x}".format(c) for c in cmd)) + else: + log.d("write:" + " ".join("{:02x}".format(ord(c)) for c in cmd)) + self._Serial.write(cmd) + return True + + def wait_for_response(self, timeout): + self.wait_event.clear() + return self.wait_event.wait(timeout) + + def set_response(self): + self.wait_event.set() + + def make_command(self, id): + #print(DataDict[id]) + data = BoardDataDict[id].pack() + l = [FIX_HEAD, id, len(data)] + head = struct.pack("3B", *l) + body = head + data + + if assistant.is_python3(): + return body + chr(checksum(body)).encode('latin1') + else: + return body + chr(checksum(body)) + + +if __name__ == '__main__': + + mboard = Transport('com10') + if not mboard.start(): + import sys + sys.exit() + + p = mboard.request(MessageID.ID_GET_VERSION) + log.i("result=%s"%p) + print('Version =[',mboard.getDataHolder()[MessageID.ID_GET_VERSION].version.decode(), mboard.getDataHolder()[MessageID.ID_GET_VERSION].build_time.decode(),"]\r\n") + +