[MOD]: PID Control差不多完成了

libs/STM32_USB-FS-Device_Lib_V4.0.0/Libraries/STM32F10x_StdPeriph_Driver/inc/pid.h

@@ -4,23 +4,26 @@
 
 extern float g_Yaw, g_Pitch, g_Roll;     //eular
 
-#define K_P 2.57f //0.257 * 0.83 0.255
-#define K_I 0
-#define K_D 0
+#define OUTTER_LOOP_KP 2.57f //0.257 * 0.83 0.255
+#define OUTTER_LOOP_KI 0.1f
+#define OUTTER_LOOP_KD 0.5f
+
+#define INNER_LOOP_KP 0.1f
+#define INNER_LOOP_KI 0.1f
+#define INNER_LOOP_KD 0.1f
+
 #define SUM_ERRO_MAX 900
 #define SUM_ERRO_MIN -900
 
-#define PID_IMAX 300
-#define PID_IMIN -300
-
-#define KP 1
-#define KI 1
-#define KD 1
+#define PID_IMAX 30
+#define PID_IMIN -30
 
 typedef struct {
-    float Last;
+    float InnerLast;
+    float OutterLast;
     float *Feedback;
-    float Erro;
+    float *Gyro;
+    float Error;
     float p;
     float i;
     float d;

libs/STM32_USB-FS-Device_Lib_V4.0.0/Libraries/STM32F10x_StdPeriph_Driver/inc/tty.h

@@ -0,0 +1,27 @@
+#ifndef TTY_H
+#define TTY_H
+
+#define TTY_NONE()          uart_sendStr("\033[m")
+#define TTY_RED()           uart_sendStr("\033[0;32;31m")
+#define TTY_LIGHT_RED()     uart_sendStr("\033[1;31m")
+#define TTY_GREEN()         uart_sendStr("\033[0;32;32m")
+#define TTY_LIGHT_GREEN()   uart_sendStr("\033[1;32m")
+#define TTY_BLUE()          uart_sendStr("\033[46;30m")
+#define TTY_LIGHT_BLUE()    uart_sendStr("\033[1;34m")
+#define TTY_DARY_GRAY()     uart_sendStr("\033[1;30m")
+#define TTY_CYAN()          uart_sendStr("\033[0;36m")
+#define TTY_LIGHT_CYAN()    uart_sendStr("\033[1;36m")
+#define TTY_PURPLE()        uart_sendStr("\033[0;35m")
+#define TTY_LIGHT_PURPLE()  uart_sendStr("\033[1;35m")
+#define TTY_BROWN()         uart_sendStr("\033[0;33m")
+#define TTY_YELLOW()        uart_sendStr("\033[1;33m")
+#define TTY_LIGHT_GRAY()    uart_sendStr("\033[0;37m")
+#define TTY_WHITE()         uart_sendStr("\033[1;37m")
+
+#define TTY_COLORRTN()    uart_sendStr("\33[7m")
+#define TTY_CLEAR()       uart_sendStr("\33[2J")
+#define TTY_UNDERLINE()   uart_sendStr("\33[4m")
+#define TTY_FLASH()       uart_sendStr("\33[5m")
+#define TTY_OPTOFF()      uart_sendStr("\33[0m")
+
+#endif

libs/STM32_USB-FS-Device_Lib_V4.0.0/Libraries/STM32F10x_StdPeriph_Driver/src/pid.c

@@ -3,17 +3,26 @@
 #include "motor.h"
 
 void pid_SingleAxis(pid_pst temp, float setPoint) {
-    temp->Erro = setPoint - *temp->Feedback;
-
-    temp->i += temp->Erro;
+    temp->Error = setPoint - *temp->Feedback;
+//Outter Loop PID
+    temp->i += temp->Error;
     if (temp->i > PID_IMAX) temp->i = PID_IMAX;
     else if (temp->i < PID_IMIN) temp->i = PID_IMIN;
 
-    temp->d = *temp->Feedback - temp->Last;
+    temp->d = *temp->Feedback - temp->OutterLast;
 
-    temp->output = (short)(KP * (temp->Erro) + KI * temp->i + KD * temp->d);
-    temp->Last = *temp->Feedback;
-    *temp->Channel1 = temp->output;
+    temp->output = (short)(OUTTER_LOOP_KP * (temp->Error) + OUTTER_LOOP_KI * temp->i + OUTTER_LOOP_KD * temp->d);
+    temp->OutterLast = *temp->Feedback; //Save Old Data
+//Inner Loop PD
+    temp->p = temp->output + *temp->Gyro * 3.5f;
+    temp->d = *temp->Gyro - temp->InnerLast;
+    temp->output = INNER_LOOP_KP * temp->p + INNER_LOOP_KD * temp->d;
+
+    if (*temp->Channel1+temp->output > MOTOR_MAX) *temp->Channel1 = MOTOR_MAX;
+    else if (*temp->Channel1 + temp->output < MOTOR_LOW) *temp->Channel1 = MOTOR_LOW;
+    temp->InnerLast = *temp->Gyro;
+
+    *temp->Channel1 += (short)temp->output;
 }
 
 // float g_iErro, g_sumErro = 0;

src/main.c

@@ -6,6 +6,7 @@
 #include "uart.h"
 #include "wifi.h"
 #include "pid.h"
+#include "tty.h"
 
 #define Kp      100.0f      //比例增益支配率(常量)
 #define Ki      0.002f      //积分增益支配率
@@ -15,19 +16,7 @@ float g_q0 = 1, g_q1 = 0, g_q2 = 0, g_q3 = 0;   //Quaternion
 float g_exInt = 0, g_eyInt = 0, g_ezInt = 0;
 float g_Yaw, g_Pitch, g_Roll;
 
-// float Last;
-// float *Feedback;
-// float Erro;
-// float p;
-// float i;
-// float d;
-// short output;
-pid_st g_pid_roll = {
-    .Last = 0,
-    .Feedback = &g_Roll,
-    .i = 0,
-    .Channel1 = &MOTOR1,
-};
+
 
 //ms
 void delay(volatile unsigned int count) {
@@ -93,6 +82,21 @@ int main() {
 #if defined (DEBUG_PID) || defined (DEBUG_MPU6050_EULER) || defined (DEBUG_MPU6050_SOURCEDATA) || defined (DEBUG_BLDC)
     SixAxis sourceData;
 #endif
+// float Last;
+// float *Feedback;
+// float Erro;
+// float p;
+// float i;
+// float d;
+// short output;
+    pid_st g_pid_roll = {
+        .InnerLast = 0,
+        .OutterLast = 0,
+        .Feedback = &g_Roll,
+        .i = 0,
+        .Channel1 = &MOTOR1,
+        .Gyro = &sourceData.gX,
+    };
 
 //Brushless motor auto init
 #ifdef DEBUG_BLDC
@@ -128,16 +132,36 @@ int main() {
         Comput(sourceData);
 
         pid_SingleAxis(&g_pid_roll, 0);
-
-        uart_sendStr("\n\nMotor占空比:\t\t");
+        TTY_CLEAR();
+        TTY_RED();
+        uart_sendStr(" Motor占空比: ");
+        TTY_NONE();
+        TTY_BLUE();
         uart_showData(MOTOR1);
+        TTY_NONE();
 
-        uart_sendStr("\n\nRoll:\t\t");
+        uart_sendStr("\n\rRoll:\t");
         uart_Float2Char(*g_pid_roll.Feedback);
 
-        uart_sendStr("\n\nP:\t\t");
+        uart_sendStr("\tGyro:\t");
+        uart_Float2Char(*g_pid_roll.Gyro);
+
+        uart_sendStr("\n\rP:\t");
         uart_Float2Char(g_pid_roll.p);
+
+        uart_sendStr("\n\rI:\t");
+        uart_Float2Char(g_pid_roll.i);
+
+        uart_sendStr("\n\rD:\t");
+        uart_Float2Char(g_pid_roll.d);
+
+        uart_sendStr("\n\r=_=_=_=_=_=_=_=_=_=_=_=_=_=_=_=_=_=_=_=_=_=_=_=_=\n\rInner Cache:\t");
+        uart_Float2Char(g_pid_roll.InnerLast);
+        uart_sendStr("\n\rOutter Cache:\t");
+        uart_Float2Char(g_pid_roll.OutterLast);
+
         UART_CR();
+
 #endif
 
 #ifdef DEBUG_MPU6050_EULER