完善

arduino/main/main.ino

@@ -1,19 +1,22 @@
   /**
-Deng's FOC 闭环速度控制例程 测试库：SimpleFOC 2.1.1 测试硬件：灯哥开源FOC V1.0
-在串口窗口中输入：T+速度，就可以使得两个电机闭环转动
-比如让两个电机都以 10rad/s 的速度转动，则输入：T10
-在使用自己的电机时，请一定记得修改默认极对数，即 BLDCMotor(7) 中的值，设置为自己的极对数数字
-程序默认设置的供电电压为 16.8V,用其他电压供电请记得修改 voltage_power_supply , voltage_limit 变量中的值
-默认PID针对的电机是 GB6010 ，使用自己的电机需要修改PID参数，才能实现更好效果
+arduino开发环境-灯哥开源FOChttps://gitee.com/ream_d/Deng-s-foc-controller，并安装Kalman。
+FOC引脚32, 33, 25, 22    22为enable
+AS5600霍尔传感器 SDA-23 SCL-5  MPU6050六轴传感器 SDA-19 SCL-18
+本程序有两种平衡方式， FLAG_V为1时使用电压控制，为0时候速度控制。电压控制时LQR参数使用K1和K2，速度控制时LQR参数使用K3和K4
+在wifi上位机窗口中输入：TA+角度，就可以修改平衡角度
+比如让平衡角度为90度，则输入：TA90，并且会存入eeprom的位置0中 注：wifi发送命令不能过快，因为每次都会保存进eeprom
+在使用自己的电机时，请一定记得修改默认极对数，即 BLDCMotor(5) 中的值，设置为自己的极对数数字，磁铁数量/2
+程序默认设置的供电电压为 12V,用其他电压供电请记得修改 voltage_power_supply , voltage_limit 变量中的值
+默认PID针对的电机是 GB2204 ，使用自己的电机需要修改PID参数，才能实现更好效果
  */
 #include <SimpleFOC.h>
 #include "Command.h"
 #include <WiFi.h>
 #include <AsyncUDP.h> //引用以使用异步UDP
 #include <Kalman.h> // Source: https://github.com/TKJElectronics/KalmanFilter
+#include "EEPROM.h"
 Kalman kalmanZ;
 #define gyroZ_OFF -0.19
-#define balance_voltage 10   //V
 /* ----IMU Data---- */
 
 double accX, accY, accZ;
@@ -42,11 +45,7 @@ unsigned int localUdpPort = 2333; //本地端口号
 void wifi_print(char * s,double num);
 
 MagneticSensorI2C sensor = MagneticSensorI2C(AS5600_I2C);
-float PID_P = 1; //
-float PID_I = 0; //
-float PID_D = 0; //
 TwoWire I2Ctwo = TwoWire(1);
-PIDController angle_pid = PIDController(PID_P, PID_I, PID_D, balance_voltage * 0.7, 20000);
 LowPassFilter lpf_throttle{0.00};
 #define FLAG_V 0
 //倒立摆参数
@@ -58,21 +57,29 @@ float LQR_K2_1 = 3.49;   //平衡态
 float LQR_K2_2 = 0.26;   //
 float LQR_K2_3 = 0.15; //
 
-float LQR_K3_1 = 5.25;   //平衡态
+float LQR_K3_1 = 5.25;   //摇摆到平衡
 float LQR_K3_2 = 3.18;   //
 float LQR_K3_3 = 1.86; //
 
+float LQR_K4_1 = 2.4;   //摇摆到平衡
+float LQR_K4_2 = 1.5;   //
+float LQR_K4_3 = 1.42; //
+
 //电机参数
 BLDCMotor motor = BLDCMotor(5);
 BLDCDriver3PWM driver = BLDCDriver3PWM(32, 33, 25, 22);
 float target_velocity = 0;
-float target_angle = 90;
+float target_angle = 89.3;
 float target_voltage = 0;
-float swing_up_voltage = 2;
+float swing_up_voltage = 1.8;
+float swing_up_angle = 18;
 //命令设置
 Command comm;
 bool Motor_enable_flag = 0;
-void do_TA(char* cmd) { comm.scalar(&target_angle, cmd); }
+void do_TA(char* cmd) { comm.scalar(&target_angle, cmd);EEPROM.writeFloat(0, target_angle); }
+void do_SV(char* cmd) { comm.scalar(&swing_up_voltage, cmd); EEPROM.writeFloat(4, swing_up_voltage); }
+void do_SA(char* cmd) { comm.scalar(&swing_up_angle, cmd);EEPROM.writeFloat(8, swing_up_angle); }
+
 void do_START(char* cmd) {  wifi_flag = !wifi_flag; }
 void do_MOTOR(char* cmd)
 {  
@@ -82,7 +89,6 @@ void do_MOTOR(char* cmd)
     digitalWrite(22,LOW);
   Motor_enable_flag = !Motor_enable_flag;
 }
-void do_SW(char* cmd) { comm.scalar(&swing_up_voltage, cmd); }
 #if FLAG_V
 void do_K11(char* cmd) { comm.scalar(&LQR_K1_1, cmd); }
 void do_K12(char* cmd) { comm.scalar(&LQR_K1_2, cmd); }
@@ -91,12 +97,15 @@ void do_K21(char* cmd) { comm.scalar(&LQR_K2_1, cmd); }
 void do_K22(char* cmd) { comm.scalar(&LQR_K2_2, cmd); }
 void do_K23(char* cmd) { comm.scalar(&LQR_K2_3, cmd); }
 #else
-void do_vp(char* cmd) { comm.scalar(&motor.PID_velocity.P, cmd); }
-void do_vi(char* cmd) { comm.scalar(&motor.PID_velocity.I, cmd); }
+void do_vp(char* cmd) { comm.scalar(&motor.PID_velocity.P, cmd); EEPROM.writeFloat(12, motor.PID_velocity.P);}
+void do_vi(char* cmd) { comm.scalar(&motor.PID_velocity.I, cmd);EEPROM.writeFloat(16, motor.PID_velocity.I); }
 void do_tv(char* cmd) { comm.scalar(&target_velocity, cmd); }
 void do_K31(char* cmd) { comm.scalar(&LQR_K3_1, cmd); }
 void do_K32(char* cmd) { comm.scalar(&LQR_K3_2, cmd); }
 void do_K33(char* cmd) { comm.scalar(&LQR_K3_3, cmd); }
+void do_K41(char* cmd) { comm.scalar(&LQR_K4_1, cmd); }
+void do_K42(char* cmd) { comm.scalar(&LQR_K4_2, cmd); }
+void do_K43(char* cmd) { comm.scalar(&LQR_K4_3, cmd); }
 #endif
 
 
@@ -106,17 +115,28 @@ void onPacketCallBack(AsyncUDPPacket packet)
   da= (char*)(packet.data());
   Serial.println(da);
   comm.run(da);
-
+  EEPROM.commit();
 //  packet.print("reply data");
 }
 // instantiate the commander
 void setup() {
    Serial.begin(115200);
+   if (!EEPROM.begin(1000)) {
+    Serial.println("Failed to initialise EEPROM");
+    Serial.println("Restarting...");
+    delay(1000);
+    ESP.restart();
+  }
+
    //命令设置
  comm.add("TA",do_TA);
   comm.add("START",do_START);
    comm.add("MOTOR",do_MOTOR);
-    comm.add("SW",do_SW);
+    comm.add("SV",do_SV);
+    comm.add("SA",do_SA);
+    target_angle = EEPROM.readFloat(0);
+swing_up_voltage = EEPROM.readFloat(4);
+swing_up_angle = EEPROM.readFloat(8);
 #if FLAG_V
   comm.add("K11",do_K11);
   comm.add("K12",do_K12);
@@ -131,6 +151,11 @@ void setup() {
   comm.add("K31",do_K31);
   comm.add("K32",do_K32);
   comm.add("K33",do_K33);
+  comm.add("K41",do_K41);
+  comm.add("K42",do_K42);
+  comm.add("K43",do_K43);
+  motor.PID_velocity.P = EEPROM.readFloat(12);
+  motor.PID_velocity.I = EEPROM.readFloat(16);
 #endif
     // kalman mpu6050 init
   Wire.begin(19, 18,400000);// Set I2C frequency to 400kHz
@@ -200,7 +225,7 @@ void setup() {
   motor.controller = MotionControlType::velocity;
     //速度PI环设置
   motor.PID_velocity.P = 0.5;
-  motor.PID_velocity.I = 10;
+  motor.PID_velocity.I = 20;
 #endif
 
 
@@ -258,18 +283,18 @@ void loop() {
       gyroZangle = kalAngleZ;
       
   float pendulum_angle = constrainAngle(fmod(kalAngleZ,120)-target_angle);
-//  float pendulum_angle = constrainAngle((fmod(kalAngleZ * 3, 360.0) / 3.0 - target_angle) / 57.29578);
-#if FLAG_V
-   if (abs(pendulum_angle) < 12) // if angle small enough stabilize 0.5~30°,1.5~90°
+//  FLAG_V为1时使用电压控制，为0时候速度控制
+#if FLAG_V 
+   if (abs(pendulum_angle) < swing_up_angle) // if angle small enough stabilize 0.5~30°,1.5~90°
    {
-     target_voltage = controllerLQR(angle_pid(pendulum_angle), gyroZrate, motor.shaftVelocity());
+     target_voltage = controllerLQR(pendulum_angle, gyroZrate, motor.shaftVelocity());
 //        limit the voltage set to the motor
     if (abs(target_voltage) > motor.voltage_limit * 0.7)
       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) * 1.5;
+        target_voltage = -_sign(gyroZrate) * swing_up_voltage;
     }
 
     // set the target voltage to the motor
@@ -283,9 +308,9 @@ void loop() {
     }
 
 #else
-if (abs(pendulum_angle) < 18) // if angle small enough stabilize 0.5~30°,1.5~90°
+if (abs(pendulum_angle) < swing_up_angle) // if angle small enough stabilize 0.5~30°,1.5~90°
    {
-  target_velocity = LQR_K3_1*pendulum_angle+LQR_K3_2*gyroZrate+LQR_K3_3*motor.shaftVelocity();
+   target_velocity = controllerLQR(pendulum_angle, gyroZrate, motor.shaftVelocity());
   if (abs(target_velocity) > 140)
       target_velocity = _sign(target_velocity) * 140;
       motor.controller = MotionControlType::velocity;
@@ -348,7 +373,7 @@ double acc2rotation(double x, double y)
   }
 }
 
-// function constraining the angle in between -pi and pi, in degrees -180 and 180
+// function constraining the angle in between -60~60
 float constrainAngle(float x)
 {
   float a = 0;
@@ -370,7 +395,7 @@ float controllerLQR(float p_angle, float p_vel, float m_vel)
   //  - k = [40, 7, 0.3]
   //  - k = [13.3, 21, 0.3]
   //  - x = [pendulum angle, pendulum velocity, motor velocity]'
-  if (abs(p_angle) > 1.5)
+  if (abs(p_angle) > 2.5)
   {
     last_unstable_time = millis();
     stable = 0;
@@ -382,6 +407,7 @@ float controllerLQR(float p_angle, float p_vel, float m_vel)
 
   //Serial.println(stable);
   float u;
+#if FLAG_V
   if (!stable)
   {
     u = LQR_K1_1 * p_angle + LQR_K1_2 * p_vel + LQR_K1_3 * m_vel;
@@ -391,8 +417,17 @@ float controllerLQR(float p_angle, float p_vel, float m_vel)
     //u = LQR_K1 * p_angle + LQR_K2 * p_vel + LQR_K3 * m_vel;
     u = LQR_K2_1 * p_angle + LQR_K2_2 * p_vel + LQR_K2_3 * m_vel;
   }
-
-
+#else
+  if (!stable)
+  {
+    u = LQR_K3_1 * p_angle + LQR_K3_2 * p_vel + LQR_K3_3 * m_vel;
+  }
+  else
+  {
+    //u = LQR_K1 * p_angle + LQR_K2 * p_vel + LQR_K3 * m_vel;
+    u = LQR_K4_1 * p_angle + LQR_K4_2 * p_vel + LQR_K4_3 * m_vel;
+  }
+#endif
   return u;
 }
 void wifi_print(char * s,double num)

arduino/problems and reasons.txt

@@ -8,4 +8,7 @@
 解决：sprintf(s, "%d", 100//将100转为10进制表示的字符串
 
 问题4：强制类型转换(const unsigned char*)不能直接使用print输出
-解决：atoi((char*)(packet.data())) //使用(char*) 进行强制类型转换
+解决：atoi((char*)(packet.data())) //使用(char*) 进行强制类型转换
+
+问题5: udp发送数据时会出现乱码
+解决：字符数组末尾需要添加'\0'作为结束符

main.cpp

@@ -1,428 +0,0 @@
-
-#include <Arduino.h>
-#include <Wire.h>
-#include <Kalman.h> // Source: https://github.com/TKJElectronics/KalmanFilter
-#define gyroZ_OFF -0.22
-
-//#define stable_angle 178.2
-//#define stable_angle 58.8
-
-//#define stable_angle 301.75
-#define stable_angle 60.0
-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
-
-/********************************************************************************/
-#include <SimpleFOC.h>
-//#include "common/foc_utils.h"
-#define swing_up_voltage 1.5 //V
-#define balance_voltage 10   //V
-#define min_voltage 9.5      //V
-/*
-  #define PID_P 0  //
-  #define PID_I 0   //
-  #define PID_D 1   //
-  #define LQR_K1 1  //
-  #define LQR_K2 0  //
-  #define LQR_K3 0.0  //
-*/
-float PID_P = 1; //
-float PID_I = 0; //
-float PID_D = 0; //
-/*
-//稳定器参数
-float LQR_K1 = 50;  //摇摆到平衡
-float LQR_K2 = 2;   //
-float LQR_K3 = 0.30; //
-
-float LQR_K1_1 = 50;   //平衡态
-float LQR_K2_1 = 2;   //
-float LQR_K3_1 = 0.15; //
-*/
-//倒立摆参数
-float LQR_K1 = 200;  //摇摆到平衡
-float LQR_K2 = 40;   //
-float LQR_K3 = 0.30; //
-
-float LQR_K1_1 = 200;   //平衡态
-float LQR_K2_1 = 15;   //
-float LQR_K3_1 = 0.15; //
-
-/*
-float LQR_K1 = 200;   //
-float LQR_K2 = 40;   //
-float LQR_K3 = 0.30; //
-*/
-/*单角度稳定
-
-float LQR_K1 = 80;  //平衡完成
-float LQR_K2 = 15;  //
-float LQR_K3 = 0.15;  //
-*/
-float OFFSET = 0;
-bool stable = 0, battery_low = 0;
-uint32_t last_unstable_time;
-//output=LQR_K1*PID+LQR_K2*p_vel + LQR_K3 * m_vel
-
-MagneticSensorI2C sensor = MagneticSensorI2C(AS5600_I2C);
-PIDController angle_pid = PIDController(PID_P, PID_I, PID_D, balance_voltage * 0.7, 20000);
-LowPassFilter lpf_throttle{0.00};
-// BLDC motor init
-BLDCMotor motor = BLDCMotor(5);
-// driver instance
-BLDCDriver3PWM driver = BLDCDriver3PWM(9, 10, 11, 8, 3);
-double rotationshift(double origin, double theta, double shift, bool y);
-double acc2rotation(double x, double y);
-float controllerLQR(float p_angle, float p_vel, float m_vel);
-float constrainAngle(float x);
-
-// 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 onj(char *cmd) { command.scalar(&LQR_K1, cmd); }
-void onk(char *cmd) { command.scalar(&LQR_K2, cmd); }
-void onl(char *cmd) { command.scalar(&LQR_K3, cmd); }
-
-/********************************************************************************/
-void setup()
-{
-  Serial.begin(115200);
-  Wire.begin();
-
-  Wire.setClock(400000UL); // Set I2C frequency to 400kHz
-  Serial.println(((analogRead(A3) / 41.5)));
-  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]);
-
-  // Source: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf eq. 25 and eq. 26
-  // atan2 outputs the value of -π to π (radians) - see http://en.wikipedia.org/wiki/Atan2
-  // It is then converted from radians to degrees
-  // Eq. 25 and 26
-
-  double pitch = acc2rotation(accX, accY);
-
-  kalmanZ.setAngle(pitch);
-  gyroZangle = pitch;
-
-  timer = micros();
-
-  pinMode(4, OUTPUT);
-  digitalWrite(4, 1);
-  sensor.init(&Wire);
-  motor.linkSensor(&sensor);
-  // driver
-  driver.voltage_power_supply = 12;
-  driver.init();
-
-  // link the driver and the motor
-  motor.linkDriver(&driver);
-
-  // aligning voltage
-  motor.voltage_sensor_align = 3;
-
-  // choose FOC modulation (optional)
-  //motor.foc_modulation = FOCModulationType::SinePWM;
-  motor.foc_modulation = FOCModulationType::SpaceVectorPWM;
-
-  // set control loop type to be used
-  motor.controller = MotionControlType::torque;
-  //motor.controller = TorqueControlType::voltage;
-
-  motor.voltage_limit = balance_voltage;
-
-  motor.useMonitoring(Serial);
-  // initialize motor
-  motor.init();
-  // align encoder and start FOC
-  //motor.initFOC(4.5,Direction::CW);
-  //motor.initFOC(4.05, Direction::CCW);
-  motor.initFOC();
-  //motor.initFOC(2.6492,Direction::CW);
-  //command.add('p', onp, "p");
-  //command.add('i', oni, "i");
-  //command.add('d', ond, "d");
-  command.add('j', onj, "newj:");
-  command.add('k', onk, "newk:");
-  command.add('l', onl, "newl:");
-
-  digitalWrite(4, 0);
-}
-long loop_count = 0;
-float target_voltage;
-void loop()
-{
-
-  motor.loopFOC();
-
-  if (loop_count++ == 10)
-  {
-    /* Update all the values */
-    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();
-
-    // Source: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf eq. 25 and eq. 26
-    // atan2 outputs the value of -π to π (radians) - see http://en.wikipedia.org/wiki/Atan2
-    // It is then converted from radians to degrees
-    // Eq. 25 and 26
-
-    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;
-    //gyroXangle += kalmanX.getRate() * dt; // Calculate gyro angle using the unbiased rate
-    //gyroYangle += kalmanY.getRate() * 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;
-
-      /* Print Data */
-#if 0 // Set to 1 to activate
-  Serial.print(accX); Serial.print("\t");
-  Serial.print(accY); Serial.print("\t");
-  Serial.print(accZ); Serial.print("\t");
-
-  Serial.print(gyroX); Serial.print("\t");
-  Serial.print(gyroY); Serial.print("\t");
-  Serial.print(gyroZ); Serial.print("\t");
-
-  Serial.print("\t");
-#endif
-#if 0
-    Serial.print(pitch);
-    Serial.print("\t");
-    Serial.print(gyroZangle);
-    Serial.print("\t");
-    Serial.print(compAngleZ);
-    Serial.print("\t");
-    Serial.print(kalAngleZ);
-    Serial.print("\t");
-
-    //Serial.print("\r\n");
-#endif
-    // calculate the pendulum angle
-    //LQR_K1 = analogRead(A3) / 10.0;
-    digitalWrite(3, 1);
-    //float pendulum_angle = constrainAngle(rotationshift(kalAngleZ * 3, 180.0, -180.0+OFFSET, 0.0) / 57.29578 + M_PI);
-    //float pendulum_angle = constrainAngle((kalAngleZ - stable_angle ) / 57.29578);
-    float pendulum_angle = constrainAngle((fmod(kalAngleZ * 3, 360.0) / 3.0 - stable_angle) / 57.29578);
-    if (abs(pendulum_angle) < 0.6) // if angle small enough stabilize 0.5~30°,1.5~90°
-    {
-      //target_voltage = controllerLQR(pendulum_angle, g.gyro.z, motor.shaftVelocity());
-
-      target_voltage = controllerLQR(angle_pid(pendulum_angle), gyroZrate / 57.29578, motor.shaftVelocity());
-
-      //digitalWrite(4, 1);
-    }
-    else // else do swing-up
-    {    // sets 1.5V to the motor in order to swing up
-      target_voltage = -_sign(gyroZrate) * swing_up_voltage;
-      digitalWrite(4, 0);
-    }
-
-    // set the target voltage to the motor
-    if (accZ < -13000 && ((accX * accX + accY * accY) > (14000 * 14000)))
-    {
-      motor.move(0);
-    }
-    else
-    {
-      motor.move(lpf_throttle(target_voltage));
-    }
-    command.run();
-    // restart the counter
-    loop_count = 0;
-    //Serial.print("kangle:");
-    driver.voltage_power_supply = analogRead(A3) / 41.5;
-    //Serial.println(driver.voltage_power_supply);
-    if ((analogRead(A3) / 41.5) < min_voltage && !battery_low)
-    {
-      battery_low = 1;
-      Serial.println("battery_low!!");
-      while (battery_low)
-      {
-        motor.loopFOC();
-        motor.move(0);
-        if (millis() % 500 < 250)
-          digitalWrite(4, 1);
-        else
-          digitalWrite(4, 0);
-      }
-    }
-    //Serial.print(",fangle:");
-    //Serial.print(constrainAngle(rotationshift(kalAngleZ * 3, 180.0, -180.0+OFFSET, 0.0) / 57.29578 + M_PI));
-
-    //Serial.println(fmod(kalAngleZ * 3, 360.0) / 3.0);
-    //Serial.print(",pid:");
-    //Serial.println(accX);
-    //Serial.print(angle_pid(pendulum_angle));
-    //Serial.print(",voltage:");
-    //Serial.print(target_voltage);
-    //Serial.print(",lpf_throttle:");
-    //Serial.println(lpf_throttle(target_voltage));
-    //Serial.print(",E_gle:");
-    //Serial.print(sensor.getAngle());
-    //Serial.print(",vel:");
-    //Serial.println(sensor.getVelocity());
-  }
-  
-    
-    
-  
-}
-
-// function constraining the angle in between -pi and pi, in degrees -180 and 180
-float constrainAngle(float x)
-{
-  x = fmod(x + M_PI, _2PI);
-  if (x < 0)
-    x += _2PI;
-  return x - M_PI;
-}
-// LQR stabilization controller functions
-// calculating the voltage that needs to be set to the motor in order to stabilize the pendulum
-float controllerLQR(float p_angle, float p_vel, float m_vel)
-{
-  // if angle controllable
-  // calculate the control law
-  // LQR controller u = k*x
-  //  - k = [40, 7, 0.3]
-  //  - k = [13.3, 21, 0.3]
-  //  - x = [pendulum angle, pendulum velocity, motor velocity]'
-  if (abs(p_angle) > 0.05)
-  {
-    last_unstable_time = millis();
-    stable = 0;
-    digitalWrite(4, 0);
-  }
-  if ((millis() - last_unstable_time) > 1000)
-  {
-    stable = 1;
-    digitalWrite(4, 1);
-  }
-
-  //Serial.println(stable);
-  float u;
-  if (!stable)
-  {
-    u = LQR_K1 * p_angle + LQR_K2 * p_vel + LQR_K3 * m_vel;
-  }
-  else
-  {
-    //u = LQR_K1 * p_angle + LQR_K2 * p_vel + LQR_K3 * m_vel;
-    u = LQR_K1_1 * p_angle + LQR_K2_1 * p_vel + LQR_K3_1 * m_vel;
-  }
-
-  // limit the voltage set to the motor
-  if (abs(u) > motor.voltage_limit * 0.7)
-    u = _sign(u) * motor.voltage_limit * 0.7;
-
-  return u;
-}
-/* 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);
-  }
-}
-
-/* mpu6050加速度转换为角度
-       rotationshift(original angle,+θ,shiftθ,0 is normal,1 is reverse)
-       rotationshift(0,30)=30
-       rotationshift(20,30)=50
-       rotationshift(0,30,1)=330
-       rotationshift(20,30,1)=310
-       rotationshift(0,0,-180,0)=-180
-*/
-double rotationshift(double origin, double theta, double shift = 0, bool y = false)
-{
-  static float origin_old;
-  if (abs(origin - origin_old) > 0.1)
-    origin_old += _sign(origin - origin_old) * 0.01;
-  else
-    origin_old = origin;
-
-  if (y == 0)
-  {
-    if (origin + theta > 360)
-      return origin + theta - 360 + shift;
-    else
-    {
-      return origin + theta + shift;
-    }
-  }
-  else
-  {
-
-    if (-(origin + theta) + 360 < 0)
-      return -(origin + theta) + 360 + 360 + shift;
-    else
-    {
-      return -(origin + theta) + 360 + shift;
-    }
-  }
-}

python_gui/gui/main.py

@@ -118,6 +118,7 @@ class MyWindow(QMainWindow, Ui_MainWindow):
             recv_data = recv_data[:-1]
             recv_data = recv_data.split(',')
             """处理接受的信息"""
+            # recv_data = [40,50,60]
             for i, data in enumerate(recv_data):
                 self.re_item.append(''.join(re.split(r'[^A-Za-z]', data)))
             print(self.re_item)