上位机调参

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");

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']

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", "发送"))

python_gui/gui/main_ui.ui

@@ -174,7 +174,7 @@
       <x>10</x>
       <y>20</y>
       <width>291</width>
-      <height>111</height>
+      <height>121</height>
      </rect>
     </property>
     <property name="title">
@@ -191,10 +191,54 @@
      </property>
      <layout class="QHBoxLayout" name="horizontalLayout_4">
       <item>
-       <widget class="QLineEdit" name="wifi_command_lineEdit"/>
+       <widget class="QLineEdit" name="wifi_command_lineEdit_1"/>
       </item>
       <item>
-       <widget class="QPushButton" name="wifi_command_pushButton">
+       <widget class="QPushButton" name="wifi_command_pushButton_1">
+        <property name="text">
+         <string>发送</string>
+        </property>
+       </widget>
+      </item>
+     </layout>
+    </widget>
+    <widget class="QWidget" name="horizontalLayoutWidget_6">
+     <property name="geometry">
+      <rect>
+       <x>10</x>
+       <y>50</y>
+       <width>271</width>
+       <height>31</height>
+      </rect>
+     </property>
+     <layout class="QHBoxLayout" name="horizontalLayout_5">
+      <item>
+       <widget class="QLineEdit" name="wifi_command_lineEdit_2"/>
+      </item>
+      <item>
+       <widget class="QPushButton" name="wifi_command_pushButton_2">
+        <property name="text">
+         <string>发送</string>
+        </property>
+       </widget>
+      </item>
+     </layout>
+    </widget>
+    <widget class="QWidget" name="horizontalLayoutWidget_7">
+     <property name="geometry">
+      <rect>
+       <x>10</x>
+       <y>80</y>
+       <width>271</width>
+       <height>31</height>
+      </rect>
+     </property>
+     <layout class="QHBoxLayout" name="horizontalLayout_6">
+      <item>
+       <widget class="QLineEdit" name="wifi_command_lineEdit_3"/>
+      </item>
+      <item>
+       <widget class="QPushButton" name="wifi_command_pushButton_3">
         <property name="text">
          <string>发送</string>
         </property>