8.25

arduino/main/i2c.ino

@@ -0,0 +1,63 @@
+/* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved.
+
+ This software may be distributed and modified under the terms of the GNU
+ General Public License version 2 (GPL2) as published by the Free Software
+ Foundation and appearing in the file GPL2.TXT included in the packaging of
+ this file. Please note that GPL2 Section 2[b] requires that all works based
+ on this software must also be made publicly available under the terms of
+ the GPL2 ("Copyleft").
+
+ Contact information
+ -------------------
+
+ Kristian Lauszus, TKJ Electronics
+ Web      :  http://www.tkjelectronics.com
+ e-mail   :  kristianl@tkjelectronics.com
+ */
+
+const uint8_t IMUAddress = 0x68; // AD0 is logic low on the PCB
+const uint16_t I2C_TIMEOUT = 1000; // Used to check for errors in I2C communication
+
+uint8_t i2cWrite(uint8_t registerAddress, uint8_t data, bool sendStop) {
+  return i2cWrite(registerAddress, &data, 1, sendStop); // Returns 0 on success
+}
+
+uint8_t i2cWrite(uint8_t registerAddress, uint8_t *data, uint8_t length, bool sendStop) {
+  Wire.beginTransmission(IMUAddress);
+  Wire.write(registerAddress);
+  Wire.write(data, length);
+  uint8_t rcode = Wire.endTransmission(sendStop); // Returns 0 on success
+  if (rcode) {
+    Serial.print(F("i2cWrite failed: "));
+    Serial.println(rcode);
+  }
+  return rcode; // See: http://arduino.cc/en/Reference/WireEndTransmission
+}
+
+uint8_t i2cRead(uint8_t registerAddress, uint8_t *data, uint8_t nbytes) {
+  uint32_t timeOutTimer;
+  Wire.beginTransmission(IMUAddress);
+  Wire.write(registerAddress);
+  uint8_t rcode = Wire.endTransmission(false); // Don't release the bus
+  if (rcode) {
+    Serial.print(F("i2cRead failed: "));
+    Serial.println(rcode);
+    return rcode; // See: http://arduino.cc/en/Reference/WireEndTransmission
+  }
+  Wire.requestFrom(IMUAddress, nbytes, (uint8_t)true); // Send a repeated start and then release the bus after reading
+  for (uint8_t i = 0; i < nbytes; i++) {
+    if (Wire.available())
+      data[i] = Wire.read();
+    else {
+      timeOutTimer = micros();
+      while (((micros() - timeOutTimer) < I2C_TIMEOUT) && !Wire.available());
+      if (Wire.available())
+        data[i] = Wire.read();
+      else {
+        Serial.println(F("i2cRead timeout"));
+        return 5; // This error value is not already taken by endTransmission
+      }
+    }
+  }
+  return 0; // Success
+}

arduino/main/main.ino

@@ -9,17 +9,44 @@ Deng's FOC 闭环速度控制例程 测试库：SimpleFOC 2.1.1 测试硬件：
 #include <SimpleFOC.h>
 #include <WiFi.h>
 #include <AsyncUDP.h> //引用以使用异步UDP
+#include <Kalman.h> // Source: https://github.com/TKJElectronics/KalmanFilter
+Kalman kalmanZ;
+#define gyroZ_OFF -0.72
+/* ----IMU Data---- */
+float PID_P = 8; //
+float PID_I = 0; //
+float PID_D = 0; //
+double accX, accY, accZ;
+double gyroX, gyroY, gyroZ;
+int16_t tempRaw;
+
+double gyroZangle; // Angle calculate using the gyro only
+double compAngleZ; // Calculated angle using a complementary filter
+double kalAngleZ;  // Calculated angle using a Kalman filter
+
+uint32_t timer;
+uint8_t i2cData[14]; // Buffer for I2C data
+/* ----FOC Data---- */
+
+// driver instance
+double acc2rotation(double x, double y);
 
 const char *ssid = "esp32";
 const char *password = "12345678";
 
+
 AsyncUDP udp;                     //创建UDP对象
 unsigned int localUdpPort = 2333; //本地端口号
+unsigned int broadcastPort = localUdpPort;
 
 MagneticSensorI2C sensor = MagneticSensorI2C(AS5600_I2C);
 
-TwoWire I2Cone = TwoWire(0);
+TwoWire I2Ctwo = TwoWire(1);
 
+//倒立摆参数
+float LQR_K1 = 200;  //摇摆到平衡
+float LQR_K2 = 15;   //
+float LQR_K3 = 0.15; //
 
 //电机参数
 BLDCMotor motor = BLDCMotor(5);
@@ -28,6 +55,7 @@ BLDCDriver3PWM driver = BLDCDriver3PWM(32, 33, 25, 22);
 
 //命令设置
 int target_velocity = 0;
+int target_angle = 149;
 void onPacketCallBack(AsyncUDPPacket packet)
 {
   target_velocity = atoi((char*)(packet.data()));
@@ -38,7 +66,45 @@ void onPacketCallBack(AsyncUDPPacket packet)
 
 void setup() {
    Serial.begin(115200);
-  //wifi初始化
+
+  
+    // kalman mpu6050 init
+  Wire.begin(19, 18,400000);// Set I2C frequency to 400kHz
+  i2cData[0] = 7;    // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz
+  i2cData[1] = 0x00; // Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro filtering, 8 KHz sampling
+  i2cData[2] = 0x00; // Set Gyro Full Scale Range to ±250deg/s
+  i2cData[3] = 0x00; // Set Accelerometer Full Scale Range to ±2g
+  while (i2cWrite(0x19, i2cData, 4, false))
+    ; // Write to all four registers at once
+  while (i2cWrite(0x6B, 0x01, true))
+    ; // PLL with X axis gyroscope reference and disable sleep mode
+
+  while (i2cRead(0x75, i2cData, 1))
+    ;
+  if (i2cData[0] != 0x68)
+  { // Read "WHO_AM_I" register
+    Serial.print(F("Error reading sensor"));
+    while (1)
+      ;
+  }
+
+  delay(100); // Wait for sensor to stabilize
+
+  /* Set kalman and gyro starting angle */
+  while (i2cRead(0x3B, i2cData, 6))
+    ;
+  accX = (int16_t)((i2cData[0] << 8) | i2cData[1]);
+  accY = (int16_t)((i2cData[2] << 8) | i2cData[3]);
+  accZ = (int16_t)((i2cData[4] << 8) | i2cData[5]);
+  double pitch = acc2rotation(accX, accY);
+
+  kalmanZ.setAngle(pitch);
+  gyroZangle = pitch;
+
+  timer = micros();
+  Serial.println("kalman mpu6050 init");
+
+    //wifi初始化
   WiFi.mode(WIFI_AP);
    while(!WiFi.softAP(ssid, password)){}; //启动AP
     Serial.println("AP启动成功");
@@ -46,50 +112,40 @@ void setup() {
   {
   }
   udp.onPacket(onPacketCallBack); //注册收到数据包事件
-  I2Cone.begin(23, 5, 400000);   //SDA,SCL
+  
-
+  I2Ctwo.begin(23, 5, 400000);   //SDA,SCL
-  sensor.init(&I2Cone);
+  sensor.init(&I2Ctwo);
 
   //连接motor对象与传感器对象
   motor.linkSensor(&sensor);
 
-
   //供电电压设置 [V]
   driver.voltage_power_supply = 12;
   driver.init();
 
-
   //连接电机和driver对象
   motor.linkDriver(&driver);
 
-  
   //FOC模型选择
   motor.foc_modulation = FOCModulationType::SpaceVectorPWM;
 
   //运动控制模式设置
   motor.controller = MotionControlType::velocity;
 
-
-
   //速度PI环设置
   motor.PID_velocity.P = 1.5;
-
   motor.PID_velocity.I = 20;
 
   //最大电机限制电机
   motor.voltage_limit = 12;
 
-  
   //速度低通滤波时间常数
   motor.LPF_velocity.Tf = 0.01;
 
-
   //设置最大速度限制
   motor.velocity_limit = 40;
 
-
   motor.useMonitoring(Serial);
-
   
   //初始化电机
   motor.init();
@@ -97,18 +153,95 @@ void setup() {
   //初始化 FOC
   motor.initFOC();
 
-
   Serial.println(F("Motor ready."));
   Serial.println(F("Set the target velocity using serial terminal:"));
-  
+
+
 }
-
+char s[255];
-
+int t_v;
-
+int lim_v = 20;
 void loop() {
+  while (i2cRead(0x3B, i2cData, 14));
+    accX = (int16_t)((i2cData[0] << 8) | i2cData[1]);
+    accY = (int16_t)((i2cData[2] << 8) | i2cData[3]);
+    accZ = (int16_t)((i2cData[4] << 8) | i2cData[5]);
+    tempRaw = (int16_t)((i2cData[6] << 8) | i2cData[7]);
+    gyroX = (int16_t)((i2cData[8] << 8) | i2cData[9]);
+    gyroY = (int16_t)((i2cData[10] << 8) | i2cData[11]);
+    gyroZ = (int16_t)((i2cData[12] << 8) | i2cData[13]);
+    
+    double dt = (double)(micros() - timer) / 1000000; // Calculate delta time
+    timer = micros();
+    
+    double pitch = acc2rotation(accX, accY);
+    double gyroZrate = gyroZ / 131.0; // Convert to deg/s
+
+    kalAngleZ = kalmanZ.getAngle(pitch, gyroZrate + gyroZ_OFF, dt);
+    gyroZangle += (gyroZrate + gyroZ_OFF) * dt;
+    compAngleZ = 0.93 * (compAngleZ + (gyroZrate + gyroZ_OFF) * dt) + 0.07 * pitch;
+
+    // Reset the gyro angle when it has drifted too much
+    if (gyroZangle < -180 || gyroZangle > 180)
+      gyroZangle = kalAngleZ;
+      
+  sprintf(s, "%.2f",kalAngleZ); //将100转为16进制表示的字符串
+//  target_velocity = -angle_pid(kalAngleZ);
+//  if (abs(target_velocity)>lim_v)
+//    target_velocity = -target_velocity;
+//  if (target_velocity >lim_v)
+//    target_velocity = lim_v;
+//  if (target_velocity<-lim_v)
+//    target_velocity = -lim_v;
+
+  Serial.print(motor.shaft_velocity);Serial.print("\t");
+  Serial.print(target_velocity);Serial.print("\t");
+  Serial.print(target_angle);Serial.print("\t");
+  Serial.print(kalAngleZ);Serial.print("\t");
+  Serial.print("\r\n");
   motor.loopFOC();
-
-
   motor.move(target_velocity);
-
+  //可以使用该方法广播信息
+  IPAddress broadcastAddr((~(uint32_t)WiFi.subnetMask())|((uint32_t)WiFi.localIP())); //计算广播地址
+  udp.writeTo((const unsigned char*)s, strlen(s), broadcastAddr, localUdpPort); //广播数据
+}
+/* mpu6050加速度转换为角度
+            acc2rotation(ax, ay)
+            acc2rotation(az, ay) */
+double acc2rotation(double x, double y)
+{
+  if (y < 0)
+  {
+    return atan(x / y) / 1.570796 * 90 + 180;
+  }
+  else if (x < 0)
+  {
+    return (atan(x / y) / 1.570796 * 90 + 360);
+  }
+  else
+  {
+    return (atan(x / y) / 1.570796 * 90);
+  }
+}
+
+unsigned long lastTime;
+double errSum, lastErr;
+int angle_pid(double now_angle)
+{
+   /*How long since we last calculated*/
+   unsigned long now = millis();
+   double timeChange = (double)(now - lastTime);
+  
+   /*Compute all the working error variables*/
+   double error = target_angle-now_angle;
+   errSum += (error * timeChange);
+   double dErr = (error - lastErr) / timeChange;
+  
+   /*Compute PID Output*/
+   int Output = PID_P * error + PID_I * errSum + PID_D * dErr;
+  
+   /*Remember some variables for next time*/
+   lastErr = error;
+   lastTime = now;
+   return Output;
 }

arduino/udp_kalman/i2c.ino

@@ -0,0 +1,63 @@
+/* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved.
+
+ This software may be distributed and modified under the terms of the GNU
+ General Public License version 2 (GPL2) as published by the Free Software
+ Foundation and appearing in the file GPL2.TXT included in the packaging of
+ this file. Please note that GPL2 Section 2[b] requires that all works based
+ on this software must also be made publicly available under the terms of
+ the GPL2 ("Copyleft").
+
+ Contact information
+ -------------------
+
+ Kristian Lauszus, TKJ Electronics
+ Web      :  http://www.tkjelectronics.com
+ e-mail   :  kristianl@tkjelectronics.com
+ */
+
+const uint8_t IMUAddress = 0x68; // AD0 is logic low on the PCB
+const uint16_t I2C_TIMEOUT = 1000; // Used to check for errors in I2C communication
+
+uint8_t i2cWrite(uint8_t registerAddress, uint8_t data, bool sendStop) {
+  return i2cWrite(registerAddress, &data, 1, sendStop); // Returns 0 on success
+}
+
+uint8_t i2cWrite(uint8_t registerAddress, uint8_t *data, uint8_t length, bool sendStop) {
+  Wire.beginTransmission(IMUAddress);
+  Wire.write(registerAddress);
+  Wire.write(data, length);
+  uint8_t rcode = Wire.endTransmission(sendStop); // Returns 0 on success
+  if (rcode) {
+    Serial.print(F("i2cWrite failed: "));
+    Serial.println(rcode);
+  }
+  return rcode; // See: http://arduino.cc/en/Reference/WireEndTransmission
+}
+
+uint8_t i2cRead(uint8_t registerAddress, uint8_t *data, uint8_t nbytes) {
+  uint32_t timeOutTimer;
+  Wire.beginTransmission(IMUAddress);
+  Wire.write(registerAddress);
+  uint8_t rcode = Wire.endTransmission(false); // Don't release the bus
+  if (rcode) {
+    Serial.print(F("i2cRead failed: "));
+    Serial.println(rcode);
+    return rcode; // See: http://arduino.cc/en/Reference/WireEndTransmission
+  }
+  Wire.requestFrom(IMUAddress, nbytes, (uint8_t)true); // Send a repeated start and then release the bus after reading
+  for (uint8_t i = 0; i < nbytes; i++) {
+    if (Wire.available())
+      data[i] = Wire.read();
+    else {
+      timeOutTimer = micros();
+      while (((micros() - timeOutTimer) < I2C_TIMEOUT) && !Wire.available());
+      if (Wire.available())
+        data[i] = Wire.read();
+      else {
+        Serial.println(F("i2cRead timeout"));
+        return 5; // This error value is not already taken by endTransmission
+      }
+    }
+  }
+  return 0; // Success
+}

arduino/udp_kalman/kalman.ino

@@ -0,0 +1,98 @@
+#include <Wire.h>
+#include <Kalman.h> // Source: https://github.com/TKJElectronics/KalmanFilter
+#define gyroZ_OFF -0.72
+Kalman kalmanZ;
+/* ----IMU Data---- */
+double accX, accY, accZ;
+double gyroX, gyroY, gyroZ;
+int16_t tempRaw;
+
+double gyroZangle; // Angle calculate using the gyro only
+double compAngleZ; // Calculated angle using a complementary filter
+double kalAngleZ;  // Calculated angle using a Kalman filter
+
+uint32_t timer;
+uint8_t i2cData[14]; // Buffer for I2C data
+
+/* ----FOC Data---- */
+
+// driver instance
+double acc2rotation(double x, double y);
+void kalman_init(){
+  Wire.begin(19, 18,400000);// Set I2C frequency to 400kHz
+  i2cData[0] = 7;    // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz
+  i2cData[1] = 0x00; // Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro filtering, 8 KHz sampling
+  i2cData[2] = 0x00; // Set Gyro Full Scale Range to ±250deg/s
+  i2cData[3] = 0x00; // Set Accelerometer Full Scale Range to ±2g
+  while (i2cWrite(0x19, i2cData, 4, false))
+    ; // Write to all four registers at once
+  while (i2cWrite(0x6B, 0x01, true))
+    ; // PLL with X axis gyroscope reference and disable sleep mode
+
+  while (i2cRead(0x75, i2cData, 1))
+    ;
+  if (i2cData[0] != 0x68)
+  { // Read "WHO_AM_I" register
+    Serial.print(F("Error reading sensor"));
+    while (1)
+      ;
+  }
+
+  delay(100); // Wait for sensor to stabilize
+
+  /* Set kalman and gyro starting angle */
+  while (i2cRead(0x3B, i2cData, 6))
+    ;
+  accX = (int16_t)((i2cData[0] << 8) | i2cData[1]);
+  accY = (int16_t)((i2cData[2] << 8) | i2cData[3]);
+  accZ = (int16_t)((i2cData[4] << 8) | i2cData[5]);
+  double pitch = acc2rotation(accX, accY);
+
+  kalmanZ.setAngle(pitch);
+  gyroZangle = pitch;
+
+  timer = micros();
+}
+double kalman_read(){
+  while (i2cRead(0x3B, i2cData, 14));
+    accX = (int16_t)((i2cData[0] << 8) | i2cData[1]);
+    accY = (int16_t)((i2cData[2] << 8) | i2cData[3]);
+    accZ = (int16_t)((i2cData[4] << 8) | i2cData[5]);
+    tempRaw = (int16_t)((i2cData[6] << 8) | i2cData[7]);
+    gyroX = (int16_t)((i2cData[8] << 8) | i2cData[9]);
+    gyroY = (int16_t)((i2cData[10] << 8) | i2cData[11]);
+    gyroZ = (int16_t)((i2cData[12] << 8) | i2cData[13]);
+    
+    double dt = (double)(micros() - timer) / 1000000; // Calculate delta time
+    timer = micros();
+    
+    double pitch = acc2rotation(accX, accY);
+    double gyroZrate = gyroZ / 131.0; // Convert to deg/s
+
+    kalAngleZ = kalmanZ.getAngle(pitch, gyroZrate + gyroZ_OFF, dt);
+    gyroZangle += (gyroZrate + gyroZ_OFF) * dt;
+    compAngleZ = 0.93 * (compAngleZ + (gyroZrate + gyroZ_OFF) * dt) + 0.07 * pitch;
+
+    // Reset the gyro angle when it has drifted too much
+    if (gyroZangle < -180 || gyroZangle > 180)
+      gyroZangle = kalAngleZ;
+    return kalAngleZ;
+  }
+/* mpu6050加速度转换为角度
+            acc2rotation(ax, ay)
+            acc2rotation(az, ay) */
+double acc2rotation(double x, double y)
+{
+  if (y < 0)
+  {
+    return atan(x / y) / 1.570796 * 90 + 180;
+  }
+  else if (x < 0)
+  {
+    return (atan(x / y) / 1.570796 * 90 + 360);
+  }
+  else
+  {
+    return (atan(x / y) / 1.570796 * 90);
+  }
+}

arduino/udp_kalman/udp_kalman.ino

@@ -0,0 +1,68 @@
+#include <WiFi.h>
+#include <AsyncUDP.h> //引用以使用异步UDP
+#include<Wire.h>
+
+const char *ssid = "esp32";
+const char *password = "12345678";
+
+double kalZ;
+AsyncUDP udp;                     //创建UDP对象
+unsigned int localUdpPort = 2333; //本地端口号
+
+
+unsigned int broadcastPort = localUdpPort;
+//const char *broadcastData = "broadcast data";
+ const uint8_t broadcastData[] = {"broadcast data"};
+
+void onPacketCallBack(AsyncUDPPacket packet)
+{
+  Serial.print("UDP数据包来源类型: ");
+  Serial.println(packet.isBroadcast() ? "广播数据" : (packet.isMulticast() ? "组播" : "单播"));
+  Serial.print("远端地址及端口号: ");
+  Serial.print(packet.remoteIP());
+  Serial.print(":");
+  Serial.println(packet.remotePort());
+  Serial.print("目标地址及端口号: ");
+  Serial.print(packet.localIP());
+  Serial.print(":");
+  Serial.println(packet.localPort());
+  Serial.print("数据长度: ");
+  Serial.println(packet.length());
+  Serial.print("数据内容: ");
+  Serial.write(packet.data(), packet.length());
+  Serial.println();
+
+//  packet.print("reply data");
+//  broadcastPort = packet.remotePort();
+}
+
+void setup()
+{
+     Serial.begin(115200);
+    // kalman mpu6050 init
+    kalman_init();
+
+  //wifi初始化
+  WiFi.mode(WIFI_AP);
+   while(!WiFi.softAP(ssid, password)){}; //启动AP
+    Serial.println("AP启动成功");
+  while (!udp.listen(localUdpPort)) //等待udp监听设置成功
+  {
+  }
+  udp.onPacket(onPacketCallBack); //注册收到数据包事件
+}
+char s[255];
+void loop()
+{
+   kalZ = kalman_read();
+   Serial.print(kalZ);Serial.print("\t");
+   sprintf(s, "%.2f", kalZ); //将100转为16进制表示的字符串
+//   Serial.println(strlen(s));
+
+  delay(10);
+
+//  udp.broadcastTo(broadcastData, broadcastPort); //可以使用该方法广播信息
+
+   IPAddress broadcastAddr((~(uint32_t)WiFi.subnetMask())|((uint32_t)WiFi.localIP())); //计算广播地址
+   udp.writeTo((const unsigned char*)s, strlen(s), broadcastAddr, localUdpPort); //广播数据
+}

python_gui/gui/main.py

@@ -7,6 +7,14 @@ import numpy as np
 import pyqtgraph as pg
 
 
+RED_COLOR = (255, 92, 92)
+GREEN_COLOR = (57, 217, 138)
+BLUE_COLOR = (91, 141, 236)
+ORANGE_COLOR = (253, 172, 66)
+YELLOW_COLOR = (255,255,51)
+PURPLE_COLOR = (75,0,130)
+MAROON_COLOR = (222,184,135)
+
 class MyWindow(QMainWindow, Ui_MainWindow):
     def __init__(self, parent=None):
         super(MyWindow, self).__init__(parent)
@@ -31,7 +39,7 @@ class MyWindow(QMainWindow, Ui_MainWindow):
         # 定时器-绘图刷新
         self.timer = QtCore.QTimer()
         self.timer.timeout.connect(self.update_plot)
-        self.timer.start(50)
+        self.timer.start(20)
         # wifi udp
         self.udp = udp()
         # self.wifi_IP_lineEdit.setText(self.udp.user_ip)
@@ -43,21 +51,39 @@ class MyWindow(QMainWindow, Ui_MainWindow):
         self.now_velocity = 0
         self.close_flag = 1
     def plot_init(self):
-        # 图表可视化数组
-        self.v_list = []
-        self.v_list.append(np.zeros(300))
-        self.timeArray = np.arange(-300, 0, 1)
         # 绘图对象
-        self.pw = pg.PlotWidget()
+        self.plotWidget = pg.PlotWidget()
-        self.curve = self.pw.plot(pen='y')
+        self.plotWidget.showGrid(x=True, y=True, alpha=0.5)
-        self.gridLayout.addWidget(self.pw)
+        # 图表可视化数组
+        self.signals = ['now_Angle', 'tag_Angle']
+        self.signal_tooltip = ['now_Angle ', 'tag_Angle']
+        self.signalColors = [RED_COLOR, BLUE_COLOR]
+        self.signalIcons = ['reddot', 'bluedot']
+        self.numberOfSamples = 300
+        self.signalDataArrays = []
+        self.signalPlots = []
+        self.signalPlotFlags = []
+        self.timeArray = np.arange(-self.numberOfSamples, 0, 1)
+        for (sig, sigColor, tooltip) in zip(self.signals, self.signalColors, self.signal_tooltip):
+            # define signal plot data array
+            self.signalDataArrays.append(np.zeros(self.numberOfSamples))
+            # configure signal plot parameters
+            signalPen = pg.mkPen(color=sigColor, width=1.5)
+            self.signalPlots.append(pg.PlotDataItem(self.timeArray,
+                                            self.signalDataArrays[-1],
+                                            pen=signalPen, name=tooltip))
+            self.plotWidget.addItem(self.signalPlots[-1])
+            # is plotted flag
+            self.signalPlotFlags.append(True)
+
+        self.gridLayout.addWidget(self.plotWidget)
 
     # 滑条绑定
     def velocity_horizontalSlider_valueChanged(self):
         self.target_velocity = self.velocity_horizontalSlider.value()
         self.velocity_lineEdit.setText(str(self.target_velocity))
-        self.udp.send_message(str(self.velocity_lineEdit))
+        self.udp.send_message(str(self.target_velocity))
-
+        print(str(self.target_velocity))
     def wifi_config_pushButton_clicked(self):
         try:
             print(self.wifi_IP_lineEdit.text(),type(self.wifi_IP_lineEdit.text()))
@@ -82,9 +108,15 @@ class MyWindow(QMainWindow, Ui_MainWindow):
             self.udp_data = recv_data
     def update_plot(self):
         if self.wifi_recv_flag:
-            self.v_list[0] = np.roll(self.v_list[0], -1)
+            self.signalDataArrays[0] = np.roll(self.signalDataArrays[0], -1)
-            self.v_list[0][-1] = self.udp_data
+            self.signalDataArrays[0][-1] = self.udp_data
-            self.curve.setData(self.timeArray, self.v_list[0])  # 在绘图部件中绘制折线图
+            self.signalDataArrays[1] = np.roll(self.signalDataArrays[1], -1)
+            self.signalDataArrays[1][-1] = self.angle_lineEdit.text()
+            # self.curve.setData(self.timeArray, self.v_list[0])  # 在绘图部件中绘制折线图
+            for i, plotFlag in enumerate(self.signalPlotFlags):
+                self.signalPlots[i].setData(self.timeArray, self.signalDataArrays[i])
+                self.signalPlots[i].updateItems()
+                self.signalPlots[i].sigPlotChanged.emit(self.signalPlots[i])
 
     def closeEvent(self, a0: QtGui.QCloseEvent) -> None:
         self.close_flag = 0

python_gui/gui/main_ui.py

@@ -14,23 +14,9 @@ from PyQt5 import QtCore, QtGui, QtWidgets
 class Ui_MainWindow(object):
     def setupUi(self, MainWindow):
         MainWindow.setObjectName("MainWindow")
-        MainWindow.resize(800, 600)
+        MainWindow.resize(800, 749)
         self.centralwidget = QtWidgets.QWidget(MainWindow)
         self.centralwidget.setObjectName("centralwidget")
-        self.velocity_horizontalSlider = QtWidgets.QSlider(self.centralwidget)
-        self.velocity_horizontalSlider.setGeometry(QtCore.QRect(470, 420, 261, 22))
-        self.velocity_horizontalSlider.setMinimum(-100)
-        self.velocity_horizontalSlider.setMaximum(100)
-        self.velocity_horizontalSlider.setOrientation(QtCore.Qt.Horizontal)
-        self.velocity_horizontalSlider.setTickPosition(QtWidgets.QSlider.TicksAbove)
-        self.velocity_horizontalSlider.setTickInterval(10)
-        self.velocity_horizontalSlider.setObjectName("velocity_horizontalSlider")
-        self.velocity_lineEdit = QtWidgets.QLineEdit(self.centralwidget)
-        self.velocity_lineEdit.setGeometry(QtCore.QRect(580, 460, 113, 21))
-        self.velocity_lineEdit.setObjectName("velocity_lineEdit")
-        self.label = QtWidgets.QLabel(self.centralwidget)
-        self.label.setGeometry(QtCore.QRect(520, 460, 51, 16))
-        self.label.setObjectName("label")
         self.gridLayoutWidget = QtWidgets.QWidget(self.centralwidget)
         self.gridLayoutWidget.setGeometry(QtCore.QRect(180, 20, 581, 361))
         self.gridLayoutWidget.setObjectName("gridLayoutWidget")
@@ -57,6 +43,38 @@ class Ui_MainWindow(object):
         self.wifi_recv_open_pushButton.setEnabled(False)
         self.wifi_recv_open_pushButton.setGeometry(QtCore.QRect(180, 380, 93, 28))
         self.wifi_recv_open_pushButton.setObjectName("wifi_recv_open_pushButton")
+        self.horizontalLayoutWidget_2 = QtWidgets.QWidget(self.centralwidget)
+        self.horizontalLayoutWidget_2.setGeometry(QtCore.QRect(570, 440, 104, 31))
+        self.horizontalLayoutWidget_2.setObjectName("horizontalLayoutWidget_2")
+        self.horizontalLayout_2 = QtWidgets.QHBoxLayout(self.horizontalLayoutWidget_2)
+        self.horizontalLayout_2.setContentsMargins(0, 0, 0, 0)
+        self.horizontalLayout_2.setObjectName("horizontalLayout_2")
+        self.label = QtWidgets.QLabel(self.horizontalLayoutWidget_2)
+        self.label.setObjectName("label")
+        self.horizontalLayout_2.addWidget(self.label)
+        self.velocity_lineEdit = QtWidgets.QLineEdit(self.horizontalLayoutWidget_2)
+        self.velocity_lineEdit.setObjectName("velocity_lineEdit")
+        self.horizontalLayout_2.addWidget(self.velocity_lineEdit)
+        self.velocity_horizontalSlider = QtWidgets.QSlider(self.centralwidget)
+        self.velocity_horizontalSlider.setGeometry(QtCore.QRect(410, 420, 261, 22))
+        self.velocity_horizontalSlider.setMinimum(-150)
+        self.velocity_horizontalSlider.setMaximum(150)
+        self.velocity_horizontalSlider.setOrientation(QtCore.Qt.Horizontal)
+        self.velocity_horizontalSlider.setTickPosition(QtWidgets.QSlider.TicksAbove)
+        self.velocity_horizontalSlider.setTickInterval(10)
+        self.velocity_horizontalSlider.setObjectName("velocity_horizontalSlider")
+        self.horizontalLayoutWidget_3 = QtWidgets.QWidget(self.centralwidget)
+        self.horizontalLayoutWidget_3.setGeometry(QtCore.QRect(400, 440, 134, 31))
+        self.horizontalLayoutWidget_3.setObjectName("horizontalLayoutWidget_3")
+        self.horizontalLayout_3 = QtWidgets.QHBoxLayout(self.horizontalLayoutWidget_3)
+        self.horizontalLayout_3.setContentsMargins(0, 0, 0, 0)
+        self.horizontalLayout_3.setObjectName("horizontalLayout_3")
+        self.label_2 = QtWidgets.QLabel(self.horizontalLayoutWidget_3)
+        self.label_2.setObjectName("label_2")
+        self.horizontalLayout_3.addWidget(self.label_2)
+        self.angle_lineEdit = QtWidgets.QLineEdit(self.horizontalLayoutWidget_3)
+        self.angle_lineEdit.setObjectName("angle_lineEdit")
+        self.horizontalLayout_3.addWidget(self.angle_lineEdit)
         MainWindow.setCentralWidget(self.centralwidget)
         self.menubar = QtWidgets.QMenuBar(MainWindow)
         self.menubar.setGeometry(QtCore.QRect(0, 0, 800, 26))
@@ -72,9 +90,11 @@ class Ui_MainWindow(object):
     def retranslateUi(self, MainWindow):
         _translate = QtCore.QCoreApplication.translate
         MainWindow.setWindowTitle(_translate("MainWindow", "MainWindow"))
-        self.velocity_lineEdit.setText(_translate("MainWindow", "0"))
-        self.label.setText(_translate("MainWindow", "速度："))
         self.groupBox.setTitle(_translate("MainWindow", "wifi_IP"))
         self.wifi_IP_lineEdit.setText(_translate("MainWindow", "192.168.4.2"))
         self.wifi_config_pushButton.setText(_translate("MainWindow", "设置"))
         self.wifi_recv_open_pushButton.setText(_translate("MainWindow", "打开"))
+        self.label.setText(_translate("MainWindow", "速度："))
+        self.velocity_lineEdit.setText(_translate("MainWindow", "0"))
+        self.label_2.setText(_translate("MainWindow", "目标角度："))
+        self.angle_lineEdit.setText(_translate("MainWindow", "149"))

python_gui/gui/main_ui.ui

@@ -7,64 +7,13 @@
     <x>0</x>
     <y>0</y>
     <width>800</width>
-    <height>600</height>
+    <height>749</height>
    </rect>
   </property>
   <property name="windowTitle">
    <string>MainWindow</string>
   </property>
   <widget class="QWidget" name="centralwidget">
-   <widget class="QSlider" name="velocity_horizontalSlider">
-    <property name="geometry">
-     <rect>
-      <x>470</x>
-      <y>420</y>
-      <width>261</width>
-      <height>22</height>
-     </rect>
-    </property>
-    <property name="minimum">
-     <number>-100</number>
-    </property>
-    <property name="maximum">
-     <number>100</number>
-    </property>
-    <property name="orientation">
-     <enum>Qt::Horizontal</enum>
-    </property>
-    <property name="tickPosition">
-     <enum>QSlider::TicksAbove</enum>
-    </property>
-    <property name="tickInterval">
-     <number>10</number>
-    </property>
-   </widget>
-   <widget class="QLineEdit" name="velocity_lineEdit">
-    <property name="geometry">
-     <rect>
-      <x>580</x>
-      <y>460</y>
-      <width>113</width>
-      <height>21</height>
-     </rect>
-    </property>
-    <property name="text">
-     <string>0</string>
-    </property>
-   </widget>
-   <widget class="QLabel" name="label">
-    <property name="geometry">
-     <rect>
-      <x>520</x>
-      <y>460</y>
-      <width>51</width>
-      <height>16</height>
-     </rect>
-    </property>
-    <property name="text">
-     <string>速度：</string>
-    </property>
-   </widget>
    <widget class="QWidget" name="gridLayoutWidget">
     <property name="geometry">
      <rect>
@@ -134,6 +83,83 @@
      <string>打开</string>
     </property>
    </widget>
+   <widget class="QWidget" name="horizontalLayoutWidget_2">
+    <property name="geometry">
+     <rect>
+      <x>570</x>
+      <y>440</y>
+      <width>104</width>
+      <height>31</height>
+     </rect>
+    </property>
+    <layout class="QHBoxLayout" name="horizontalLayout_2">
+     <item>
+      <widget class="QLabel" name="label">
+       <property name="text">
+        <string>速度：</string>
+       </property>
+      </widget>
+     </item>
+     <item>
+      <widget class="QLineEdit" name="velocity_lineEdit">
+       <property name="text">
+        <string>0</string>
+       </property>
+      </widget>
+     </item>
+    </layout>
+   </widget>
+   <widget class="QSlider" name="velocity_horizontalSlider">
+    <property name="geometry">
+     <rect>
+      <x>410</x>
+      <y>420</y>
+      <width>261</width>
+      <height>22</height>
+     </rect>
+    </property>
+    <property name="minimum">
+     <number>-150</number>
+    </property>
+    <property name="maximum">
+     <number>150</number>
+    </property>
+    <property name="orientation">
+     <enum>Qt::Horizontal</enum>
+    </property>
+    <property name="tickPosition">
+     <enum>QSlider::TicksAbove</enum>
+    </property>
+    <property name="tickInterval">
+     <number>10</number>
+    </property>
+   </widget>
+   <widget class="QWidget" name="horizontalLayoutWidget_3">
+    <property name="geometry">
+     <rect>
+      <x>400</x>
+      <y>440</y>
+      <width>134</width>
+      <height>31</height>
+     </rect>
+    </property>
+    <layout class="QHBoxLayout" name="horizontalLayout_3">
+     <item>
+      <widget class="QLabel" name="label_2">
+       <property name="text">
+        <string>目标角度：</string>
+       </property>
+      </widget>
+     </item>
+     <item>
+      <widget class="QLineEdit" name="angle_lineEdit">
+       <property name="text">
+        <string>149</string>
+       </property>
+      </widget>
+     </item>
+    </layout>
+   </widget>
   </widget>
   <widget class="QMenuBar" name="menubar">
    <property name="geometry">