增量PID移植完成

Core/Src/gpio.c

@@ -134,7 +134,7 @@ void MX_GPIO_Init(void)
   LL_GPIO_SetPinMode(MOTOR_A_GPIO_Port, MOTOR_A_Pin, LL_GPIO_MODE_FLOATING);
 
   /* EXTI interrupt init*/
-  NVIC_SetPriority(EXTI0_IRQn, NVIC_EncodePriority(NVIC_GetPriorityGrouping(),0, 0));
+  NVIC_SetPriority(EXTI0_IRQn, NVIC_EncodePriority(NVIC_GetPriorityGrouping(),4, 0));
   NVIC_EnableIRQ(EXTI0_IRQn);
 
 }

Core/Src/stm32f1xx_it.c

@@ -69,8 +69,8 @@
 /*           Cortex-M3 Processor Interruption and Exception Handlers          */
 /******************************************************************************/
 /**
- * @brief This function handles Non maskable interrupt.
+  * @brief This function handles Non maskable interrupt.
- */
+  */
 void NMI_Handler(void)
 {
   /* USER CODE BEGIN NonMaskableInt_IRQn 0 */
@@ -84,8 +84,8 @@ void NMI_Handler(void)
 }
 
 /**
- * @brief This function handles Hard fault interrupt.
+  * @brief This function handles Hard fault interrupt.
- */
+  */
 void HardFault_Handler(void)
 {
   /* USER CODE BEGIN HardFault_IRQn 0 */
@@ -99,8 +99,8 @@ void HardFault_Handler(void)
 }
 
 /**
- * @brief This function handles Memory management fault.
+  * @brief This function handles Memory management fault.
- */
+  */
 void MemManage_Handler(void)
 {
   /* USER CODE BEGIN MemoryManagement_IRQn 0 */
@@ -114,8 +114,8 @@ void MemManage_Handler(void)
 }
 
 /**
- * @brief This function handles Prefetch fault, memory access fault.
+  * @brief This function handles Prefetch fault, memory access fault.
- */
+  */
 void BusFault_Handler(void)
 {
   /* USER CODE BEGIN BusFault_IRQn 0 */
@@ -129,8 +129,8 @@ void BusFault_Handler(void)
 }
 
 /**
- * @brief This function handles Undefined instruction or illegal state.
+  * @brief This function handles Undefined instruction or illegal state.
- */
+  */
 void UsageFault_Handler(void)
 {
   /* USER CODE BEGIN UsageFault_IRQn 0 */
@@ -144,8 +144,8 @@ void UsageFault_Handler(void)
 }
 
 /**
- * @brief This function handles System service call via SWI instruction.
+  * @brief This function handles System service call via SWI instruction.
- */
+  */
 void SVC_Handler(void)
 {
   /* USER CODE BEGIN SVCall_IRQn 0 */
@@ -157,8 +157,8 @@ void SVC_Handler(void)
 }
 
 /**
- * @brief This function handles Debug monitor.
+  * @brief This function handles Debug monitor.
- */
+  */
 void DebugMon_Handler(void)
 {
   /* USER CODE BEGIN DebugMonitor_IRQn 0 */
@@ -170,8 +170,8 @@ void DebugMon_Handler(void)
 }
 
 /**
- * @brief This function handles Pendable request for system service.
+  * @brief This function handles Pendable request for system service.
- */
+  */
 void PendSV_Handler(void)
 {
   /* USER CODE BEGIN PendSV_IRQn 0 */
@@ -183,8 +183,8 @@ void PendSV_Handler(void)
 }
 
 /**
- * @brief This function handles System tick timer.
+  * @brief This function handles System tick timer.
- */
+  */
 void SysTick_Handler(void)
 {
   /* USER CODE BEGIN SysTick_IRQn 0 */
@@ -204,8 +204,8 @@ void SysTick_Handler(void)
 /******************************************************************************/
 
 /**
- * @brief This function handles EXTI line0 interrupt.
+  * @brief This function handles EXTI line0 interrupt.
- */
+  */
 void EXTI0_IRQHandler(void)
 {
   /* USER CODE BEGIN EXTI0_IRQn 0 */
@@ -224,8 +224,8 @@ void EXTI0_IRQHandler(void)
 }
 
 /**
- * @brief This function handles DMA1 channel4 global interrupt.
+  * @brief This function handles DMA1 channel4 global interrupt.
- */
+  */
 void DMA1_Channel4_IRQHandler(void)
 {
   /* USER CODE BEGIN DMA1_Channel4_IRQn 0 */
@@ -238,8 +238,8 @@ void DMA1_Channel4_IRQHandler(void)
 }
 
 /**
- * @brief This function handles DMA1 channel5 global interrupt.
+  * @brief This function handles DMA1 channel5 global interrupt.
- */
+  */
 void DMA1_Channel5_IRQHandler(void)
 {
   /* USER CODE BEGIN DMA1_Channel5_IRQn 0 */
@@ -252,8 +252,8 @@ void DMA1_Channel5_IRQHandler(void)
 }
 
 /**
- * @brief This function handles TIM1 update interrupt.
+  * @brief This function handles TIM1 update interrupt.
- */
+  */
 void TIM1_UP_IRQHandler(void)
 {
   /* USER CODE BEGIN TIM1_UP_IRQn 0 */
@@ -271,8 +271,8 @@ void TIM1_UP_IRQHandler(void)
 }
 
 /**
- * @brief This function handles TIM3 global interrupt.
+  * @brief This function handles TIM3 global interrupt.
- */
+  */
 void TIM3_IRQHandler(void)
 {
   /* USER CODE BEGIN TIM3_IRQn 0 */
@@ -281,15 +281,15 @@ void TIM3_IRQHandler(void)
   /* USER CODE BEGIN TIM3_IRQn 1 */
   if (IS_TIM_IT_FLAG(WORK_TIM))
   {
-    TIM_IRQ_HANDLER(WORK_TIM);
     work_process();
+    TIM_IRQ_HANDLER(WORK_TIM);
   }
   /* USER CODE END TIM3_IRQn 1 */
 }
 
 /**
- * @brief This function handles USART1 global interrupt.
+  * @brief This function handles USART1 global interrupt.
- */
+  */
 void USART1_IRQHandler(void)
 {
   /* USER CODE BEGIN USART1_IRQn 0 */

Core/Src/tim.c

@@ -122,7 +122,7 @@ void MX_TIM3_Init(void)
   LL_APB1_GRP1_EnableClock(LL_APB1_GRP1_PERIPH_TIM3);
 
   /* TIM3 interrupt Init */
-  NVIC_SetPriority(TIM3_IRQn, NVIC_EncodePriority(NVIC_GetPriorityGrouping(),1, 0));
+  NVIC_SetPriority(TIM3_IRQn, NVIC_EncodePriority(NVIC_GetPriorityGrouping(),3, 0));
   NVIC_EnableIRQ(TIM3_IRQn);
 
   /* USER CODE BEGIN TIM3_Init 1 */

MDK-ARM/motor.uvoptx

@@ -213,6 +213,12 @@
       <pszMrulep></pszMrulep>
       <pSingCmdsp></pSingCmdsp>
       <pMultCmdsp></pMultCmdsp>
+      <SystemViewers>
+        <Entry>
+          <Name>System Viewer\GPIOB</Name>
+          <WinId>35905</WinId>
+        </Entry>
+      </SystemViewers>
     </TargetOption>
   </Target>
 
@@ -810,7 +816,7 @@
 
   <Group>
     <GroupName>User/lib/control</GroupName>
-    <tvExp>0</tvExp>
+    <tvExp>1</tvExp>
     <tvExpOptDlg>0</tvExpOptDlg>
     <cbSel>0</cbSel>
     <RteFlg>0</RteFlg>

User/application/app.c

@@ -32,7 +32,12 @@ static uint8_t business_inspection(struct flow *fl)
             ssd1306_update_screen();
             break;
         case WORK_MOTOR_SPEED:
-            /* code */
+            ssd1306_clear_buffer();
+            ssd1306_f8x16_string(0, 0, " TAR:");
+            ssd1306_f8x16_number(48, 0, work.target_speed, 0);
+            ssd1306_f8x16_string(0, 2, " CUR:");
+            ssd1306_f8x16_number(48, 2, work.encoder_show, 0);
+            ssd1306_update_screen();
             break;
         default:
             break;

User/application/key.c

@@ -1,6 +1,8 @@
 #include "key.h"
 #include "btn.h"
 #include "work.h"
+#define INVALID_BUTTON_TICKS 200 // 无效按键时间 毫秒
+static uint32_t key_start_ticks = 0;
 /* 按钮 */
 struct Button key_1;
 struct Button key_2;
@@ -9,8 +11,24 @@ struct Button key_4;
 struct Button key_5;
 struct Button key_6;
 
+static BOOL allow_condition(void)
+{
+    /**
+     * key的初始化在LCD板子上电之前，因为是低电平有效，所以会误判为按键按下
+     */
+    if (sys_millis() - key_start_ticks < INVALID_BUTTON_TICKS) // 仿真的时候按键会有毛刺，在xx秒之后按下有效
+    {
+        return FALSE;
+    }
+    return TRUE;
+}
+
 static uint8_t read_button_gpio(button_id_e button_id)
 {
+    if (allow_condition() == FALSE)
+    {
+        return ACTIVE_LEVEL_LOW;
+    }
     switch (button_id)
     {
     case KEY_ADD:
@@ -20,7 +38,7 @@ static uint8_t read_button_gpio(button_id_e button_id)
     case KEY_S:
         return GPIO_READ(KEY_S_GPIO_Port, KEY_S_Pin);
     default:
-        return ACTIVE_LEVEL_HIGH;
+        return ACTIVE_LEVEL_LOW;
     }
 }
 
@@ -41,13 +59,14 @@ static void key_3_press_down_handler(void *btn)
 
 void key_init(void)
 {
+    key_start_ticks = sys_millis();
     GPIO_SET_INPUT(KEY_ADD_GPIO_Port, KEY_ADD_Pin);
     GPIO_SET_INPUT(KEY_SUB_GPIO_Port, KEY_SUB_Pin);
     GPIO_SET_INPUT(KEY_S_GPIO_Port, KEY_S_Pin);
 
-    button_init(&key_1, read_button_gpio, ACTIVE_LEVEL_LOW, KEY_ADD, KEY_ADD);
+    button_init(&key_1, read_button_gpio, ACTIVE_LEVEL_HIGH, KEY_ADD, KEY_ADD);
-    button_init(&key_2, read_button_gpio, ACTIVE_LEVEL_LOW, KEY_SUB, KEY_SUB);
+    button_init(&key_2, read_button_gpio, ACTIVE_LEVEL_HIGH, KEY_SUB, KEY_SUB);
-    button_init(&key_3, read_button_gpio, ACTIVE_LEVEL_LOW, KEY_S, KEY_S);
+    button_init(&key_3, read_button_gpio, ACTIVE_LEVEL_HIGH, KEY_S, KEY_S);
 
     button_attach(&key_1, PRESS_DOWN, key_1_press_down_handler);
     button_attach(&key_2, PRESS_DOWN, key_2_press_down_handler);

User/application/work.c

@@ -2,6 +2,9 @@
 #include "board.h"
 #define OUTPUT_INFORMATION_CYCLE_BASE 100
 #define DISTANCE 1000
+#define SPEED 80
+#define TARGET_POS 500
+#define TARGET_SPEED 20
 work_t work;
 
 /**
@@ -22,6 +25,7 @@ static void output_information(void)
     }
     else if (work.type == WORK_MOTOR_SPEED)
     {
+        rst = snprintf(data, ARRAY_LEN(data), "%d,%d,%f\r\n", work.target_speed, work.encoder_show, work.pwm_percent);
     }
     else
     {
@@ -44,14 +48,27 @@ static void pwm_map(void)
 
 static void motor_pos(void)
 {
-    work.pid.sub_type = PID_SUB_TYPE_POSITION;
+    int32_t code = work.encoder_cnt;
-    work.pwm_percent = work.pid.pid_u.fuzzy.execute(&work.pid.pid_u.fuzzy, work.target_pos, work.encoder_cnt);
+    work.pwm_percent = work.pid.pid_u.fuzzy.execute(&work.pid.pid_u.fuzzy, work.target_pos, code);
     set_motor_pwm(work.pwm_percent);
 }
 
 static void motor_speed(void)
 {
-    work.pid.sub_type = PID_SUB_TYPE_INCREMENT;
+    int32_t code = work.encoder_cnt;
+    if (ABS(work.target_speed - work.encoder_show) > 20)
+    {
+        work.target_speed_filter = lpf_update(&work.target_speed_lpf, work.target_speed);
+    }
+    else
+    {
+        work.target_speed_filter = work.target_speed;
+    }
+
+    work.encoder_show = lpf_update(&work.encoder_lpf, work.encoder_cnt);
+    work.encoder_cnt = 0;
+    work.pwm_percent = work.pid.pid_u.fuzzy.execute(&work.pid.pid_u.fuzzy, work.target_speed_filter + 1, code); // 补偿1
+    set_motor_pwm(work.pwm_percent);
 }
 
 static void pwm_map_key_handle(button_id_e id)
@@ -99,7 +116,7 @@ static void motot_pos_key_handle(button_id_e id)
         }
         break;
     case KEY_S:
-        work.target_pos = 500;
+        work.target_pos = TARGET_POS;
         break;
     default:
         break;
@@ -108,6 +125,28 @@ static void motot_pos_key_handle(button_id_e id)
 
 static void motor_speed_key_handle(button_id_e id)
 {
+    int16_t min = TARGET_SPEED, max = SPEED;
+    int16_t step = 5;
+    switch (id)
+    {
+    case KEY_ADD:
+        if (work.target_speed < max)
+        {
+            work.target_speed += step;
+        }
+        break;
+    case KEY_SUB:
+        if (work.target_speed > min)
+        {
+            work.target_speed -= step;
+        }
+        break;
+    case KEY_S:
+        work.target_speed = TARGET_SPEED;
+        break;
+    default:
+        break;
+    }
 }
 
 void work_key_handle_cb(button_id_e id)
@@ -162,7 +201,7 @@ void work_process(void)
 
 void work_encoder_exti(void)
 {
-    if (GPIO_READ(MOTOR_B_GPIO_Port, MOTOR_B_Pin))
+    if (GPIO_READ(MOTOR_B_GPIO_Port, MOTOR_B_Pin) == 1)
     {
         work.encoder_cnt++;
     }
@@ -179,24 +218,60 @@ void work_init(void)
     PWM_SET_DUTY(PWM_TIM, PWM_CHANNEL, 0);
     work.pwm_feq = PWM_GET_FREQ(PWM_TIM);
     work.pwm_arr = PWM_GET_ARR(PWM_TIM);
-    work.type = WORK_MOTOR_POS;
+    work.type = WORK_MOTOR_SPEED;
-    work.pid_params.kp = 1;
+
-    work.pid_params.ki = 0.01f;
-    work.pid_params.kd = 10;
-    work.pid_params.dead_zone = 0.5;
     // PID初始化
 
     {
         work.pid.type = PID_TYPE_FUZZY;
-        work.pid.sub_type = PID_SUB_TYPE_POSITION;
         work.pid.pid_u.fuzzy.deadzone_dir = DEAD_ZONE_BOTH;
+
+        work.pid_params.dead_zone = 1;
+
+        if (work.type == WORK_MOTOR_POS)
+        {
+            work.pid_params.kp = 1;
+            work.pid_params.ki = 0.01f;
+            work.pid_params.kd = 10;
+            work.pid.sub_type = PID_SUB_TYPE_POSITION;
+        }
+        else
+        {
+            work.pid_params.kp = 0.01;
+            work.pid_params.ki = 0.1f;
+            work.pid_params.kd = 0;
+            work.pid.sub_type = PID_SUB_TYPE_INCREMENT;
+        }
         pid_constructor(&work.pid);
-        work.pid.pid_u.fuzzy.set_ctrl_prm(&work.pid.pid_u.fuzzy, work.pid_params.kp, work.pid_params.ki,
+
-                                          work.pid_params.kd, work.pid_params.dead_zone, 0, -90, 90); // 电机输出不要超过90
+        if (work.type == WORK_MOTOR_POS)
-        work.pid.pid_u.fuzzy.set_kd_enable(&work.pid.pid_u.fuzzy, TRUE);
+        {
+            work.pid.pid_u.fuzzy.set_ctrl_prm(&work.pid.pid_u.fuzzy, work.pid_params.kp, work.pid_params.ki,
+                                              work.pid_params.kd, work.pid_params.dead_zone, 0, -90, 90); // 电机输出不要超过90
+            work.pid.pid_u.fuzzy.set_kd_enable(&work.pid.pid_u.fuzzy, TRUE);
+            work.position = &work.pid.pid_u.fuzzy.pri_u.position;
+        }
+        else
+        {
+            work.pid.pid_u.fuzzy.set_ctrl_prm(&work.pid.pid_u.fuzzy, work.pid_params.kp, work.pid_params.ki,
+                                              work.pid_params.kd, work.pid_params.dead_zone, 0, -1 * ((int32_t)SPEED), SPEED);
+            work.pid.pid_u.fuzzy.set_kd_enable(&work.pid.pid_u.fuzzy, FALSE);
+            work.increment = &work.pid.pid_u.fuzzy.pri_u.increment;
+            work.encoder_lpf.alpha = work.pid_params.ki * 1.5f;
+            lpf_init(&work.encoder_lpf);
+
+            work.target_speed_lpf.alpha = work.pid_params.ki * 1.5f;
+            lpf_init(&work.target_speed_lpf);
+
+            // 改变定时器周期
+            uint16_t arr = LL_TIM_GetAutoReload(WORK_TIM);
+            LL_TIM_SetAutoReload(WORK_TIM, arr * 10 - 1);
+        }
     }
 
-    work.target_pos = 500;
+    work.target_pos = TARGET_POS;
+    work.target_speed = TARGET_SPEED;
     work.timer_cycle = TIM_CYCLE(WORK_TIM);
+
     return;
 }

User/application/work.h

@@ -4,7 +4,7 @@
 #include "main.h"
 #include "key.h"
 #include "pid.h"
-
+#include "filter.h"
 typedef enum
 {
     // PWM占空比映射到电机
@@ -18,13 +18,16 @@ typedef enum
 typedef struct
 {
     work_e type;
-    uint8_t timer_cycle; // 定时器周期
+    uint8_t timer_cycle;  // 定时器周期
-    uint32_t enter_cnt;  // 进入次数
+    uint32_t enter_cnt;   // 进入次数
-    int32_t encoder_cnt; // 编码器位置
+    int32_t encoder_cnt;  // 编码器位置
-    float32 pwm_percent; // PWM占空比
+    int32_t encoder_show; // 滤波后的编码器位置
-    uint32_t pwm_feq;    // PWM频率
+    float32 pwm_percent;  // PWM占空比
-    uint16_t pwm_arr;    // PWM自动重装载值
+    uint32_t pwm_feq;     // PWM频率
+    uint16_t pwm_arr;     // PWM自动重装载值
     pid_t pid;
+    pid_common_position_t *position;
+    pid_common_increment_t *increment;
     struct
     {
         float32 kp;
@@ -34,7 +37,10 @@ typedef struct
     } pid_params;
 
     int32_t target_pos;   // 目标位置
-    float32 target_speed; // 目标速度
+    int32_t target_speed; // 目标速度
+    int32_t target_speed_filter;
+    lpf_t encoder_lpf;
+    lpf_t target_speed_lpf;
 } work_t;
 extern work_t work;
 

User/board/board.c

@@ -47,6 +47,21 @@ void set_motor_pwm(float32 percent)
     PWM_SET_DUTY(PWM_TIM, PWM_CHANNEL, ABS(percent));
 }
 
+void set_motor_pwm_arr(int16_t arr)
+{
+    if (arr > 0)
+    {
+        GPIO_SET(AIN1_GPIO_Port, AIN1_Pin);
+        GPIO_RESET(AIN2_GPIO_Port, AIN2_Pin);
+    }
+    else
+    {
+        GPIO_RESET(AIN1_GPIO_Port, AIN1_Pin);
+        GPIO_SET(AIN2_GPIO_Port, AIN2_Pin);
+    }
+    PWM_SET_COMPARE(PWM_TIM, PWM_CHANNEL, ABS(arr));
+}
+
 /**
  * @brief 通过UART发送数据
  *

User/board/board.h

@@ -15,5 +15,5 @@ extern uart_t *uart;
 void board_init(void);
 void uart_send(uint8_t *data, uint16_t len);
 void set_motor_pwm(float32 percent);
-
+void set_motor_pwm_arr(int16_t arr);
 #endif

User/lib/control/inc/pid.h

@@ -7,8 +7,8 @@
 
 typedef enum
 {
-    PID_SUB_TYPE_POSITION,  // 位置式
+    PID_SUB_TYPE_POSITION = 1,  // 位置式
-    PID_SUB_TYPE_INCREMENT, // 增量式
+    PID_SUB_TYPE_INCREMENT = 2, // 增量式
 } pid_sub_type_e;
 typedef enum
 {

User/lib/control/src/pid.c

@@ -13,6 +13,7 @@ void pid_constructor(pid_t *self)
         pid_neural_constructor(&self->pid_u.neural);
         break;
     case PID_TYPE_FUZZY:
+        DBG_ASSERT(self->sub_type != 0 __DBG_LINE);
         self->pid_u.fuzzy.sub_type = self->sub_type;
         pid_fuzzy_constructor(&self->pid_u.fuzzy);
         break;

User/lib/control/src/pid_fuzzy.c

@@ -13,17 +13,17 @@
 #define PL 3
 
 // 定义偏差E的范围，因为设置了非线性区间，误差在10时才开始进行PID调节，这里E的范围为10
-#define MAXE (10)
+#define MAXE (90)
 #define MINE (-MAXE)
 // 定义EC的范围，因为变化非常缓慢！，每次的EC都非常小，这里可以根据实际需求来调整，
-#define MAXEC (10)
+#define MAXEC (90)
 #define MINEC (-MAXEC)
 // 定义e,ec的量化因子
 #define KE 3 / MAXE
 #define KEC 3 / MAXEC
 
 // 定义输出量比例因子
-#define KUP 3.0f // 这里只使用了模糊PID的比例增益
+#define KUP 1.0f // 这里只使用了模糊PID的比例增益
 #define KUI 0.0f
 #define KUD 0.0f
 

motor.ioc

@@ -71,7 +71,7 @@ NVIC.BusFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false
 NVIC.DMA1_Channel4_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true
 NVIC.DMA1_Channel5_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true
 NVIC.DebugMonitor_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false
-NVIC.EXTI0_IRQn=true\:0\:0\:false\:false\:true\:true\:true\:true
+NVIC.EXTI0_IRQn=true\:4\:0\:true\:false\:true\:true\:true\:true
 NVIC.ForceEnableDMAVector=true
 NVIC.HardFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false
 NVIC.MemoryManagement_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false
@@ -81,7 +81,7 @@ NVIC.PriorityGroup=NVIC_PRIORITYGROUP_4
 NVIC.SVCall_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false
 NVIC.SysTick_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:false
 NVIC.TIM1_UP_IRQn=true\:3\:0\:true\:false\:true\:true\:true\:true
-NVIC.TIM3_IRQn=true\:1\:0\:true\:false\:true\:true\:true\:true
+NVIC.TIM3_IRQn=true\:3\:0\:true\:false\:true\:true\:true\:true
 NVIC.USART1_IRQn=true\:5\:0\:true\:false\:true\:true\:true\:true
 NVIC.UsageFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false
 PA0-WKUP.GPIOParameters=GPIO_Label