9.15

arduino/main/main.ino

@@ -36,10 +36,11 @@ float constrainAngle(float x);
 const char *ssid = "esp32";
 const char *password = "12345678";
 
-
+#define WIFI_FLAG 1
 AsyncUDP udp;                     //创建UDP对象
 unsigned int localUdpPort = 2333; //本地端口号
 unsigned int broadcastPort = localUdpPort;
+void wifi_print(char * s,double num);
 
 MagneticSensorI2C sensor = MagneticSensorI2C(AS5600_I2C);
 float PID_P = 1; //
@@ -50,7 +51,7 @@ 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 = 600;  //摇摆到平衡
+float LQR_K1 = 400;  //摇摆到平衡
 float LQR_K2 = 40;   //
 float LQR_K3 = 0.30; //
 
@@ -65,7 +66,7 @@ BLDCDriver3PWM driver = BLDCDriver3PWM(32, 33, 25, 22);
 
 //命令设置
 double target_velocity = 0;
-double target_angle = 31;
+double target_angle = 88;
 double target_voltage = 0;
 void onPacketCallBack(AsyncUDPPacket packet)
 {
@@ -75,10 +76,6 @@ void onPacketCallBack(AsyncUDPPacket packet)
 //  packet.print("reply data");
 }
 // instantiate the commander
-Commander command = Commander(Serial);
-void onp(char* cmd) { command.scalar(&PID_P, cmd); }
-void oni(char* cmd) { command.scalar(&PID_I, cmd); }
-void ond(char* cmd) { command.scalar(&PID_D, cmd); }
 void setup() {
    Serial.begin(115200);
 
@@ -172,21 +169,18 @@ void setup() {
   //初始化 FOC
   motor.initFOC();
 
-      command.add('P', onp, "newKP");
-    command.add('I', oni, "newKI");
-    command.add('D', ond, "newKD");
   Serial.println(F("Motor ready."));
   Serial.println(F("Set the target velocity using serial terminal:"));
 
 
 }
-char s[255];
+char buf[255];
 int t_v;
 int lim_v = 60;
 long loop_count = 0;
 void loop() {
     motor.loopFOC();
-  if (loop_count++ == 5)
+  if (1)
   {
   while (i2cRead(0x3B, i2cData, 14));
     accX = (int16_t)((i2cData[0] << 8) | i2cData[1]);
@@ -211,19 +205,25 @@ void loop() {
     if (gyroZangle < -180 || gyroZangle > 180)
       gyroZangle = kalAngleZ;
       
-  sprintf(s, "%.2f",kalAngleZ); //将100转为16进制表示的字符串
+
   float pendulum_angle = constrainAngle((fmod(kalAngleZ * 3, 360.0) / 3.0 - target_angle) / 57.29578);
 #if FLAG_V
-   if (abs(pendulum_angle) < 0.3) // if angle small enough stabilize 0.5~30°,1.5~90°
+   if (abs(pendulum_angle) < 0.6) // if angle small enough stabilize 0.5~30°,1.5~90°
    {
      target_voltage = controllerLQR(angle_pid(pendulum_angle), gyroZrate / 57.29578, motor.shaftVelocity());
 //        limit the voltage set to the motor
-    if (abs(target_voltage) > motor.voltage_limit * 0.9)
+    if (abs(target_voltage) > motor.voltage_limit * 0.7)
-      target_voltage = _sign(target_voltage) * motor.voltage_limit * 0.9;
+      target_voltage = _sign(target_voltage) * motor.voltage_limit * 0.7;
    }
     else // else do swing-up
     {    // sets 1.5V to the motor in order to swing up
-      target_voltage = -_sign(gyroZrate) * swing_up_voltage;
+      if(loop_count == 0)
+      {
+        loop_count = 30;
+        target_voltage = -_sign(gyroZrate) * 3;
+      }
+      else
+        loop_count--;
     }
 
     // set the target voltage to the motor
@@ -235,7 +235,7 @@ void loop() {
     {
       motor.move(lpf_throttle(target_voltage));
     }
-     Serial.print(target_voltage);Serial.print("\t");
+
 #else
     if (abs(pendulum_angle) < 0.6) // if angle small enough stabilize 0.5~30°,1.5~90°
    {
@@ -258,9 +258,11 @@ void loop() {
     {
       motor.move(lpf_throttle(target_velocity));
     }
-      Serial.print(target_velocity);Serial.print("\t");
+      
 #endif
-#if 1
+#if 0
+  Serial.print(target_voltage);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");
@@ -269,11 +271,17 @@ void loop() {
 #endif
 //  motor.move(target_velocity);
   //可以使用该方法广播信息
+#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())); //计算广播地址
-  udp.writeTo((const unsigned char*)s, strlen(s), broadcastAddr, localUdpPort); //广播数据
+  udp.writeTo((const unsigned char*)buf, strlen(buf), broadcastAddr, localUdpPort); //广播数据
-  loop_count = 0;
   }
-  command.run();
+#endif
 }
 /* mpu6050加速度转换为角度
             acc2rotation(ax, ay)
@@ -336,3 +344,13 @@ float controllerLQR(float p_angle, float p_vel, float m_vel)
 
   return u;
 }
+void wifi_print(char * s,double num)
+{
+  char str[255];
+  char n[255];
+  sprintf(n, "%.2f",num);
+  strcpy(str,s);
+  strcat(str, n);
+  strcat(buf+strlen(buf), str);
+  strcat(buf, ",");
+}

python_gui/gui/main.py

@@ -105,18 +105,23 @@ class MyWindow(QMainWindow, Ui_MainWindow):
         while self.close_flag:
             recv_data = self.udp.udpClientSocket.recv(1024)
             recv_data = recv_data.decode('utf-8')
-            self.udp_data = recv_data
+            recv_data = recv_data[:-1]
+            self.udp_data = recv_data.split(',')
+            """处理接受的信息"""
+            print(len(self.udp_data))
+            for i, data in enumerate(self.udp_data):
+                # print(i,data)
+
+                # self.signalDataArrays[i] = np.roll(self.signalDataArrays[i], -1)
+                # self.signalDataArrays[i][-1] = data
+                pass
     def update_plot(self):
-        if self.wifi_recv_flag:
+        pass
-            self.signalDataArrays[0] = np.roll(self.signalDataArrays[0], -1)
+        # if self.wifi_recv_flag:
-            self.signalDataArrays[0][-1] = self.udp_data
+        #     for i, plotFlag in enumerate(self.signalPlotFlags):
-            self.signalDataArrays[1] = np.roll(self.signalDataArrays[1], -1)
+        #         self.signalPlots[i].setData(self.timeArray, self.signalDataArrays[i])
-            self.signalDataArrays[1][-1] = self.angle_lineEdit.text()
+        #         self.signalPlots[i].updateItems()
-            # self.curve.setData(self.timeArray, self.v_list[0])  # 在绘图部件中绘制折线图
+        #         self.signalPlots[i].sigPlotChanged.emit(self.signalPlots[i])
-            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/test1.py

@@ -1,7 +0,0 @@
-import socket
-#创建socket对象
-#SOCK_DGRAM  udp模式
-s=socket.socket(socket.AF_INET,socket.SOCK_DGRAM)
-s.bind(("169.254.184.146",8000)) #绑定服务器的ip和端口
-data=s.recv(1024) #一次接收1024字节
-print(data.decode())# decode()解码收到的字节