diff --git a/arduino/main/main.ino b/arduino/main/main.ino index 0d7d577..ead87fe 100644 --- a/arduino/main/main.ino +++ b/arduino/main/main.ino @@ -51,13 +51,13 @@ PIDController angle_pid = PIDController(PID_P, PID_I, PID_D, balance_voltage * 0 LowPassFilter lpf_throttle{0.00}; #define FLAG_V 1 //倒立摆参数 -float LQR_K1 = 0; //摇摆到平衡 -float LQR_K2 = 0; // -float LQR_K3 = 0; // +float LQR_K1 = 4; //摇摆到平衡 +float LQR_K2 = 1.5; // +float LQR_K3 = 0.30; // -float LQR_K1_1 = 0; //平衡态 -float LQR_K2_1 = 0; // -float LQR_K3_1 = 0; // +float LQR_K1_1 = 3.49; //平衡态 +float LQR_K2_1 = 0.26; // +float LQR_K3_1 = 0.15; // //电机参数 @@ -201,9 +201,10 @@ int lim_v = 60; long loop_count = 0; void loop() { motor.loopFOC(); - if (loop_count++ == 10) + if (1) { - loop_count = 0; +// loop_count++ == 10 +// loop_count = 0; while (i2cRead(0x3B, i2cData, 14)); accX = (int16_t)((i2cData[0] << 8) | i2cData[1]); accY = (int16_t)((i2cData[2] << 8) | i2cData[3]); @@ -239,7 +240,7 @@ void loop() { } else // else do swing-up { // sets 1.5V to the motor in order to swing up - target_voltage = -_sign(gyroZrate) * 3; + target_voltage = -_sign(gyroZrate) * 2; } // set the target voltage to the motor @@ -255,7 +256,7 @@ void loop() { #else motor.move(0); #endif -#if 0 +#if 1 //Serial.print(gyroZangle);Serial.print("\t"); Serial.print(kalAngleZ);Serial.print("\t"); diff --git a/python_gui/gui/main.py b/python_gui/gui/main.py index e8748b3..47c37d2 100644 --- a/python_gui/gui/main.py +++ b/python_gui/gui/main.py @@ -28,7 +28,9 @@ class MyWindow(QMainWindow, Ui_MainWindow): def CreateSignalSlot(self): self.velocity_horizontalSlider.valueChanged.connect(self.velocity_horizontalSlider_valueChanged) self.wifi_config_pushButton.clicked.connect(self.wifi_config_pushButton_clicked) - self.wifi_command_pushButton.clicked.connect(self.wifi_command_pushButton_clicked) + self.wifi_command_pushButton_1.clicked.connect(self.wifi_command_pushButton_1_clicked) + self.wifi_command_pushButton_2.clicked.connect(self.wifi_command_pushButton_2_clicked) + self.wifi_command_pushButton_3.clicked.connect(self.wifi_command_pushButton_3_clicked) # 设置实例 def CreateItems(self): # 定时器-绘图刷新 @@ -80,8 +82,12 @@ class MyWindow(QMainWindow, Ui_MainWindow): self.udp.send_message(str(self.target_velocity)) print(str(self.target_velocity)) # command命令发送事件 - def wifi_command_pushButton_clicked(self): - self.udp.send_message(self.wifi_command_lineEdit.text()) + def wifi_command_pushButton_1_clicked(self): + self.udp.send_message(self.wifi_command_lineEdit_1.text()) + def wifi_command_pushButton_2_clicked(self): + self.udp.send_message(self.wifi_command_lineEdit_2.text()) + def wifi_command_pushButton_3_clicked(self): + self.udp.send_message(self.wifi_command_lineEdit_3.text()) def wifi_config_pushButton_clicked(self): try: # self.re_item = ['k','g','l','t'] diff --git a/python_gui/gui/main_ui.py b/python_gui/gui/main_ui.py index 4e32497..56f582f 100644 --- a/python_gui/gui/main_ui.py +++ b/python_gui/gui/main_ui.py @@ -2,7 +2,7 @@ # Form implementation generated from reading ui file 'main_ui.ui' # -# Created by: PyQt5 UI code generator 5.15.2 +# Created by: PyQt5 UI code generator 5.15.4 # # 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. @@ -81,7 +81,7 @@ class Ui_MainWindow(object): self.tool_layout.setContentsMargins(0, 0, 0, 0) self.tool_layout.setObjectName("tool_layout") self.groupBox_3 = QtWidgets.QGroupBox(self.centralwidget) - self.groupBox_3.setGeometry(QtCore.QRect(10, 20, 291, 111)) + self.groupBox_3.setGeometry(QtCore.QRect(10, 20, 291, 121)) self.groupBox_3.setObjectName("groupBox_3") self.horizontalLayoutWidget_5 = QtWidgets.QWidget(self.groupBox_3) self.horizontalLayoutWidget_5.setGeometry(QtCore.QRect(10, 20, 271, 31)) @@ -89,12 +89,36 @@ class Ui_MainWindow(object): self.horizontalLayout_4 = QtWidgets.QHBoxLayout(self.horizontalLayoutWidget_5) self.horizontalLayout_4.setContentsMargins(0, 0, 0, 0) self.horizontalLayout_4.setObjectName("horizontalLayout_4") - self.wifi_command_lineEdit = QtWidgets.QLineEdit(self.horizontalLayoutWidget_5) - self.wifi_command_lineEdit.setObjectName("wifi_command_lineEdit") - self.horizontalLayout_4.addWidget(self.wifi_command_lineEdit) - self.wifi_command_pushButton = QtWidgets.QPushButton(self.horizontalLayoutWidget_5) - self.wifi_command_pushButton.setObjectName("wifi_command_pushButton") - self.horizontalLayout_4.addWidget(self.wifi_command_pushButton) + self.wifi_command_lineEdit_1 = QtWidgets.QLineEdit(self.horizontalLayoutWidget_5) + self.wifi_command_lineEdit_1.setObjectName("wifi_command_lineEdit_1") + self.horizontalLayout_4.addWidget(self.wifi_command_lineEdit_1) + self.wifi_command_pushButton_1 = QtWidgets.QPushButton(self.horizontalLayoutWidget_5) + self.wifi_command_pushButton_1.setObjectName("wifi_command_pushButton_1") + self.horizontalLayout_4.addWidget(self.wifi_command_pushButton_1) + self.horizontalLayoutWidget_6 = QtWidgets.QWidget(self.groupBox_3) + self.horizontalLayoutWidget_6.setGeometry(QtCore.QRect(10, 50, 271, 31)) + self.horizontalLayoutWidget_6.setObjectName("horizontalLayoutWidget_6") + self.horizontalLayout_5 = QtWidgets.QHBoxLayout(self.horizontalLayoutWidget_6) + self.horizontalLayout_5.setContentsMargins(0, 0, 0, 0) + self.horizontalLayout_5.setObjectName("horizontalLayout_5") + self.wifi_command_lineEdit_2 = QtWidgets.QLineEdit(self.horizontalLayoutWidget_6) + self.wifi_command_lineEdit_2.setObjectName("wifi_command_lineEdit_2") + self.horizontalLayout_5.addWidget(self.wifi_command_lineEdit_2) + self.wifi_command_pushButton_2 = QtWidgets.QPushButton(self.horizontalLayoutWidget_6) + self.wifi_command_pushButton_2.setObjectName("wifi_command_pushButton_2") + self.horizontalLayout_5.addWidget(self.wifi_command_pushButton_2) + self.horizontalLayoutWidget_7 = QtWidgets.QWidget(self.groupBox_3) + self.horizontalLayoutWidget_7.setGeometry(QtCore.QRect(10, 80, 271, 31)) + self.horizontalLayoutWidget_7.setObjectName("horizontalLayoutWidget_7") + self.horizontalLayout_6 = QtWidgets.QHBoxLayout(self.horizontalLayoutWidget_7) + self.horizontalLayout_6.setContentsMargins(0, 0, 0, 0) + self.horizontalLayout_6.setObjectName("horizontalLayout_6") + self.wifi_command_lineEdit_3 = QtWidgets.QLineEdit(self.horizontalLayoutWidget_7) + self.wifi_command_lineEdit_3.setObjectName("wifi_command_lineEdit_3") + self.horizontalLayout_6.addWidget(self.wifi_command_lineEdit_3) + self.wifi_command_pushButton_3 = QtWidgets.QPushButton(self.horizontalLayoutWidget_7) + self.wifi_command_pushButton_3.setObjectName("wifi_command_pushButton_3") + self.horizontalLayout_6.addWidget(self.wifi_command_pushButton_3) MainWindow.setCentralWidget(self.centralwidget) self.menubar = QtWidgets.QMenuBar(MainWindow) self.menubar.setGeometry(QtCore.QRect(0, 0, 1187, 26)) @@ -119,4 +143,6 @@ class Ui_MainWindow(object): self.angle_lineEdit.setText(_translate("MainWindow", "149")) self.groupBox_2.setTitle(_translate("MainWindow", "GroupBox")) self.groupBox_3.setTitle(_translate("MainWindow", "command命令")) - self.wifi_command_pushButton.setText(_translate("MainWindow", "发送")) + self.wifi_command_pushButton_1.setText(_translate("MainWindow", "发送")) + self.wifi_command_pushButton_2.setText(_translate("MainWindow", "发送")) + self.wifi_command_pushButton_3.setText(_translate("MainWindow", "发送")) diff --git a/python_gui/gui/main_ui.ui b/python_gui/gui/main_ui.ui index 7626a06..84d2a29 100644 --- a/python_gui/gui/main_ui.ui +++ b/python_gui/gui/main_ui.ui @@ -174,7 +174,7 @@ 10 20 291 - 111 + 121 @@ -191,10 +191,54 @@ - + - + + + 发送 + + + + + + + + + 10 + 50 + 271 + 31 + + + + + + + + + + 发送 + + + + + + + + + 10 + 80 + 271 + 31 + + + + + + + + 发送