位置式完成

User/application/app.c

@@ -24,7 +24,12 @@ static uint8_t business_inspection(struct flow *fl)
             ssd1306_update_screen();
             break;
         case WORK_MOTOR_POS:
-            /* code */
+            ssd1306_clear_buffer();
+            ssd1306_f8x16_string(0, 0, " TAR:");
+            ssd1306_f8x16_number(48, 0, work.target_pos, 1);
+            ssd1306_f8x16_string(0, 2, " CUR:");
+            ssd1306_f8x16_number(48, 2, work.encoder_cnt, 1);
+            ssd1306_update_screen();
             break;
         case WORK_MOTOR_SPEED:
             /* code */

User/application/work.c

@@ -11,6 +11,8 @@ static void pwm_map(void)
 static void motor_pos(void)
 {
     work.pid.sub_type = PID_SUB_TYPE_POSITION;
+    work.pwm_percent = work.pid.pid_u.fuzzy.execute(&work.pid.pid_u.fuzzy, work.target_pos, work.encoder_cnt);
+    set_motor_pwm(work.pwm_percent);
 }
 
 static void motor_speed(void)
@@ -46,6 +48,33 @@ static void pwm_map_key_handle(button_id_e id)
 
 static void motot_pos_key_handle(button_id_e id)
 {
+    int8_t min = -100, max = 100;
+    int8_t step = 10;
+    switch (id)
+    {
+    case KEY_ADD:
+        if (work.target_pos < max)
+        {
+            work.target_pos += step;
+        }
+        break;
+    case KEY_SUB:
+        if (work.target_pos > min)
+        {
+            work.target_pos -= step;
+        }
+        break;
+    case KEY_S:
+        work.target_pos = 50;
+        break;
+    default:
+        break;
+    }
+
+    if (work.target_pos == 0)
+    {
+        work.encoder_cnt = 0;
+    }
 }
 
 static void motor_speed_key_handle(button_id_e id)
@@ -110,11 +139,11 @@ void work_init(void)
     PWM_SET_DUTY(PWM_TIM, PWM_CHANNEL, 0);
     work.pwm_feq = PWM_GET_FREQ(PWM_TIM);
 
-    work.type = WORK_PWM_MAP;
+    work.type = WORK_MOTOR_POS;
-    work.pid_params.kp = 100;
+    work.pid_params.kp = 1;
-    work.pid_params.ki = 0.01;
+    work.pid_params.ki = 0.01f;
-    work.pid_params.kd = 50;
+    work.pid_params.kd = 5;
-    work.pid_params.dead_zone = 0;
+    work.pid_params.dead_zone = 0.5f;
     // PID初始化
 
     {
@@ -123,8 +152,10 @@ void work_init(void)
         work.pid.pid_u.fuzzy.deadzone_dir = DEAD_ZONE_BOTH;
         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, -100, 100);
+                                          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.target_pos = 20;
     return;
 }

User/application/work.h

@@ -23,7 +23,6 @@ typedef struct
     uint32_t pwm_feq;    // PWM频率
 
     pid_t pid;
-
     struct
     {
         float32 kp;
@@ -31,6 +30,10 @@ typedef struct
         float32 kd;
         float32 dead_zone;
     } pid_params;
+
+    float32 target_pos;   // 目标位置
+    float32 target_speed; // 目标速度
+
 } work_t;
 extern work_t work;