forked from logzhan/RobotHardware-UESTC
更新UPbot可视化配置工具
parent
6142e60a99
commit
1828a774bb
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
|
|
@ -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_())
|
||||
|
|
@ -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(),
|
||||
}
|
||||
|
||||
|
|
@ -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)
|
||||
|
|
@ -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)
|
||||
|
|
@ -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"))
|
||||
File diff suppressed because it is too large
Load Diff
|
|
@ -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
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
|
|
@ -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<b
|
||||
self.section.append((a,b))
|
||||
def removeSection(self,a,b):
|
||||
a,b=int(a),int(b)
|
||||
self.section.remove((a,b))
|
||||
def removeAllSectioin(self):
|
||||
self.section=[]
|
||||
def inSection(self):
|
||||
tick=self.ticker
|
||||
for (a,b) in self.section:
|
||||
if tick>=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
|
||||
|
|
@ -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)
|
||||
|
|
@ -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().
|
||||
"""
|
||||
|
|
@ -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")
|
||||
|
||||
|
|
@ -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.level<level:return
|
||||
try:
|
||||
if len(args)>0:
|
||||
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)
|
||||
|
|
@ -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)
|
||||
|
|
@ -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
|
||||
|
|
@ -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)
|
||||
|
|
@ -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)
|
||||
|
|
@ -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")
|
||||
|
||||
|
||||
Loading…
Reference in New Issue