9.27完成摇摆平衡，还需要调参数

arduino/main/main.ino

@@ -11,7 +11,7 @@ Deng's FOC 闭环速度控制例程 测试库：SimpleFOC 2.1.1 测试硬件：
 #include <AsyncUDP.h> //引用以使用异步UDP
 #include <Kalman.h> // Source: https://github.com/TKJElectronics/KalmanFilter
 Kalman kalmanZ;
-#define gyroZ_OFF -0.72
+#define gyroZ_OFF -0.52
 #define swing_up_voltage 5 //V
 #define balance_voltage 10   //V
 /* ----IMU Data---- */
@@ -36,10 +36,9 @@ float constrainAngle(float x);
 const char *ssid = "esp32";
 const char *password = "12345678";
 
-#define WIFI_FLAG 1
+bool wifi_flag = 0;
 AsyncUDP udp;                     //创建UDP对象
 unsigned int localUdpPort = 2333; //本地端口号
-unsigned int broadcastPort = localUdpPort;
 void wifi_print(char * s,double num);
 
 MagneticSensorI2C sensor = MagneticSensorI2C(AS5600_I2C);
@@ -52,13 +51,14 @@ LowPassFilter lpf_throttle{0.00};
 #define FLAG_V 1
 //倒立摆参数
 float LQR_K1 = 400;  //摇摆到平衡
-float LQR_K2 = 40;   //
+float LQR_K2 = 80;   //
-float LQR_K3 = 0.30; //
+float LQR_K3 = 0.50; //
 
 float LQR_K1_1 = 200;   //平衡态
 float LQR_K2_1 = 15;   //
 float LQR_K3_1 = 0.15; //
 
+
 //电机参数
 BLDCMotor motor = BLDCMotor(5);
 BLDCDriver3PWM driver = BLDCDriver3PWM(32, 33, 25, 22);
@@ -66,13 +66,15 @@ BLDCDriver3PWM driver = BLDCDriver3PWM(32, 33, 25, 22);
 
 //命令设置
 double target_velocity = 0;
-double target_angle = 88;
+double target_angle = 91;
 double target_voltage = 0;
 void onPacketCallBack(AsyncUDPPacket packet)
 {
+ 
   target_velocity = atoi((char*)(packet.data()));
   Serial.print("数据内容: ");
   Serial.println(target_velocity);
+  wifi_flag = 1;
 //  packet.print("reply data");
 }
 // instantiate the commander
@@ -180,8 +182,9 @@ int lim_v = 60;
 long loop_count = 0;
 void loop() {
     motor.loopFOC();
-  if (1)
+  if (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]);
@@ -217,14 +220,8 @@ void loop() {
    }
     else // else do swing-up
     {    // sets 1.5V to the motor in order to swing up
-      if(loop_count == 0)
-      {
-        loop_count = 30;
         target_voltage = -_sign(gyroZrate) * 3;
     }
-      else
-        loop_count--;
-    }
 
     // set the target voltage to the motor
     if (accZ < -13000 && ((accX * accX + accY * accY) > (14000 * 14000)))
@@ -261,8 +258,11 @@ void loop() {
       
 #endif
 #if 1
+
+Serial.print(kalAngleZ);Serial.print("\t");
+
   Serial.print(target_voltage);Serial.print("\t");
-  Serial.print(target_velocity);Serial.print("\t");
+//  Serial.print(target_velocity);Serial.print("\t");
   Serial.print(motor.shaft_velocity);Serial.print("\t");
   Serial.print(target_angle);Serial.print("\t");
   Serial.print(pendulum_angle+target_angle);Serial.print("\t");
@@ -271,17 +271,21 @@ void loop() {
 #endif
 //  motor.move(target_velocity);
   //可以使用该方法广播信息
-#if WIFI_FLAG
+if(wifi_flag)
-
+{
   memset(buf, 0, strlen(buf));  
   wifi_print("p",pendulum_angle+target_angle);
   wifi_print("t",target_angle);
   wifi_print("k",kalAngleZ);
   wifi_print("g",gyroZrate);
-  IPAddress broadcastAddr((~(uint32_t)WiFi.subnetMask())|((uint32_t)WiFi.localIP())); //计算广播地址
+//  IPAddress broadcastAddr("192.168.4.255")
-  udp.writeTo((const unsigned char*)buf, strlen(buf), broadcastAddr, localUdpPort); //广播数据
+//IPAddress broadcastAddr(((uint32_t)"192.168.4.2")); //计算广播地址
+//  IPAddress broadcastAddr((~(uint32_t)WiFi.subnetMask())|((uint32_t)WiFi.localIP())); //计算广播地址
+//  Serial.println(buf);
+//const char * udpAddress = "192.168.4.255";
+  udp.writeTo((const unsigned char*)buf, strlen(buf), IPAddress(192,168,4,2), localUdpPort); //广播数据
+  }
 }
-#endif
 }
 /* mpu6050加速度转换为角度
             acc2rotation(ax, ay)

python_gui/gui/main.py

@@ -81,24 +81,25 @@ class MyWindow(QMainWindow, Ui_MainWindow):
         print(str(self.target_velocity))
     def wifi_config_pushButton_clicked(self):
         try:
-            self.re_item = ['k','g','l','t']
+            # self.re_item = ['k','g','l','t']
-            self.plot_init()
-            # print(self.wifi_IP_lineEdit.text(),type(self.wifi_IP_lineEdit.text()))
-            # self.udp.udpClientSocket.bind((self.wifi_IP_lineEdit.text(), 2333))
-            # # 第一次接受数据，用于判断项目数，
-            # recv_data = self.udp.udpClientSocket.recv(1024)
-            # recv_data = recv_data.decode('utf-8')
-            # recv_data = recv_data[:-1]
-            # recv_data = recv_data.split(',')
-            # """处理接受的信息"""
-            # for i, data in enumerate(recv_data):
-            #     self.re_item.append(''.join(re.split(r'[^A-Za-z]', data)))
-            # print(self.re_item)
-            # # 图表初始化
             # self.plot_init()
-            # t1 = threading.Thread(target=self.udp_recv)
+
-            # t1.start()
+            print(self.wifi_IP_lineEdit.text(),type(self.wifi_IP_lineEdit.text()))
-            # self.wifi_recv_open_pushButton.setEnabled(True)
+            self.udp.udpClientSocket.bind((self.wifi_IP_lineEdit.text(), 2333))
+            # 第一次接受数据，用于判断项目数，
+            self.udp.send_message("s")
+            recv_data = self.udp.udpClientSocket.recv(1024)
+            recv_data = recv_data.decode('utf-8')
+            recv_data = recv_data[:-1]
+            recv_data = recv_data.split(',')
+            """处理接受的信息"""
+            for i, data in enumerate(recv_data):
+                self.re_item.append(''.join(re.split(r'[^A-Za-z]', data)))
+            print(self.re_item)
+            # 图表初始化
+            self.plot_init()
+            t1 = threading.Thread(target=self.udp_recv)
+            t1.start()
         except Exception as e:
             print(e)
             QMessageBox.critical(self, "错误", '该请求的地址无效')
@@ -109,7 +110,7 @@ class MyWindow(QMainWindow, Ui_MainWindow):
             recv_data = recv_data[:-1]
             recv_data = recv_data.split(',')
             """处理接受的信息"""
-            print(recv_data)
+            # print(recv_data)
             for i, data in enumerate(recv_data):
                 self.re_item.append(''.join(re.split(r'[^A-Za-z]', data)))
                 data = re.findall(r"\d+\.?\d*", data)