待合并代码

.vscode/c_cpp_properties.json

@@ -0,0 +1,20 @@
+{
+    "configurations": [
+        {
+            "name": "Win32",
+            "includePath": [
+                "${workspaceFolder}/**"
+            ],
+            "defines": [
+                "_DEBUG",
+                "UNICODE",
+                "_UNICODE"
+            ],
+            "compilerPath": "C:\\MinGW\\bin\\gcc.exe",
+            "cStandard": "c11",
+            "cppStandard": "gnu++14",
+            "intelliSenseMode": "windows-gcc-x86"
+        }
+    ],
+    "version": 4
+}

.vscode/settings.json

@@ -0,0 +1,3 @@
+{
+  "C_Cpp.errorSquiggles": "disabled"
+}

MDK-ARM/RTE/_controller-v2/RTE_Components.h

@@ -1,6 +1,6 @@
 
 /*
- * Auto generated Run-Time-Environment Configuration File
+ * Auto generated Run-Time-Environment Component Configuration File
  *      *** Do not modify ! ***
  *
  * Project: 'controller-v2' 
@@ -17,5 +17,4 @@
 #define CMSIS_device_header "stm32l4xx.h"
 
 
-
 #endif /* RTE_COMPONENTS_H */

MDK-ARM/controller-v2.uvoptx

@@ -103,7 +103,7 @@
         <bEvRecOn>1</bEvRecOn>
         <bSchkAxf>0</bSchkAxf>
         <bTchkAxf>0</bTchkAxf>
-        <nTsel>6</nTsel>
+        <nTsel>11</nTsel>
         <sDll></sDll>
         <sDllPa></sDllPa>
         <sDlgDll></sDlgDll>
@@ -117,10 +117,15 @@
         <pMon>STLink\ST-LINKIII-KEIL_SWO.dll</pMon>
       </DebugOpt>
       <TargetDriverDllRegistry>
+        <SetRegEntry>
+          <Number>0</Number>
+          <Key>PWSTATINFO</Key>
+          <Name>200,50,700</Name>
+        </SetRegEntry>
         <SetRegEntry>
           <Number>0</Number>
           <Key>JL2CM3</Key>
-          <Name>-U601012352 -O78 -S2 -ZTIFSpeedSel5000 -A0 -C0 -JU1 -JI127.0.0.1 -JP0 -RST0 -N00("ARM CoreSight SW-DP") -D00(2BA01477) -L00(0) -TO18 -TC10000000 -TP21 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -TB1 -TFE0 -FO7 -FD20000000 -FC8000 -FN1 -FF0STM32L4xx_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32L476VGTx$CMSIS\Flash\STM32L4xx_1024.FLM)</Name>
+          <Name>-U601012352 -O14 -S2 -ZTIFSpeedSel5000 -A0 -C0 -JU1 -JI127.0.0.1 -JP0 -RST0 -N00("ARM CoreSight SW-DP") -D00(2BA01477) -L00(0) -TO18 -TC10000000 -TP21 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -TB1 -TFE0 -FO15 -FD20000000 -FC8000 -FN1 -FF0STM32L4xx_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32L476VGTx$CMSIS\Flash\STM32L4xx_1024.FLM)</Name>
         </SetRegEntry>
         <SetRegEntry>
           <Number>0</Number>
@@ -140,7 +145,7 @@
         <SetRegEntry>
           <Number>0</Number>
           <Key>ST-LINKIII-KEIL_SWO</Key>
-          <Name>-U-O206 -O206 -SF10000 -C0 -A0 -I0 -HNlocalhost -HP7184 -P1 -N00("ARM CoreSight SW-DP (ARM Core") -D00(2BA01477) -L00(0) -TO131090 -TC10000000 -TT10000000 -TP21 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO7 -FD20000000 -FC8000 -FN1 -FF0STM32L4xx_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32L476VGTx$CMSIS\Flash\STM32L4xx_1024.FLM)</Name>
+          <Name>-U73002200150000504A51544E -O206 -S1 -C0 -A0 -N00("ARM CoreSight SW-DP") -D00(2BA01477) -L00(0) -TO131090 -TC10000000 -TP21 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO15 -FD20000000 -FC8000 -FN1 -FF0STM32L4xx_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32L476VGTx$CMSIS\Flash\STM32L4xx_1024.FLM)</Name>
         </SetRegEntry>
         <SetRegEntry>
           <Number>0</Number>
@@ -163,27 +168,27 @@
         <Ww>
           <count>1</count>
           <WinNumber>1</WinNumber>
-          <ItemText>mode_control,0x0A</ItemText>
+          <ItemText>pdctrl_data,0x0A</ItemText>
         </Ww>
         <Ww>
           <count>2</count>
           <WinNumber>1</WinNumber>
-          <ItemText>mode_control_adjust,0x0A</ItemText>
+          <ItemText>_pid,0x0A</ItemText>
         </Ww>
         <Ww>
           <count>3</count>
           <WinNumber>1</WinNumber>
-          <ItemText>pdctrl_data,0x0A</ItemText>
+          <ItemText>calib_param</ItemText>
         </Ww>
         <Ww>
           <count>4</count>
           <WinNumber>1</WinNumber>
-          <ItemText>_pid</ItemText>
+          <ItemText>show_loop</ItemText>
         </Ww>
         <Ww>
           <count>5</count>
           <WinNumber>1</WinNumber>
-          <ItemText>calib_param</ItemText>
+          <ItemText>adc2_raw,0x0A</ItemText>
         </Ww>
         <Ww>
           <count>6</count>
@@ -193,22 +198,47 @@
         <Ww>
           <count>7</count>
           <WinNumber>1</WinNumber>
-          <ItemText>adc2_raw,0x0A</ItemText>
+          <ItemText>show_actual</ItemText>
         </Ww>
         <Ww>
           <count>8</count>
           <WinNumber>1</WinNumber>
-          <ItemText>show_loop</ItemText>
+          <ItemText>show_target</ItemText>
         </Ww>
         <Ww>
           <count>9</count>
           <WinNumber>1</WinNumber>
-          <ItemText>show_actual</ItemText>
+          <ItemText>mode_pwmp_hd,0x0A</ItemText>
         </Ww>
         <Ww>
           <count>10</count>
           <WinNumber>1</WinNumber>
-          <ItemText>show_target</ItemText>
+          <ItemText>mode_pwmp_hd_adjust,0x0A</ItemText>
+        </Ww>
+        <Ww>
+          <count>11</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>mode_pwmp_hd_test,0x0A</ItemText>
+        </Ww>
+        <Ww>
+          <count>12</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>pid_autotune_hd,0x0A</ItemText>
+        </Ww>
+        <Ww>
+          <count>13</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>pid_target</ItemText>
+        </Ww>
+        <Ww>
+          <count>14</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>raw_adc</ItemText>
+        </Ww>
+        <Ww>
+          <count>15</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>adc_raw,0x0A</ItemText>
         </Ww>
       </WatchWindow1>
       <MemoryWindow1>
@@ -263,7 +293,6 @@
       <pMultCmdsp></pMultCmdsp>
       <DebugDescription>
         <Enable>1</Enable>
-        <EnableFlashSeq>0</EnableFlashSeq>
         <EnableLog>0</EnableLog>
         <Protocol>2</Protocol>
         <DbgClock>10000000</DbgClock>

MDK-ARM/controller-v2.uvprojx

@@ -10,14 +10,14 @@
       <TargetName>controller-v2</TargetName>
       <ToolsetNumber>0x4</ToolsetNumber>
       <ToolsetName>ARM-ADS</ToolsetName>
-      <pCCUsed>5060960::V5.06 update 7 (build 960)::.\ARMCC</pCCUsed>
+      <pCCUsed>5060061::V5.06 update 1 (build 61)::ARMCC</pCCUsed>
       <uAC6>0</uAC6>
       <TargetOption>
         <TargetCommonOption>
           <Device>STM32L476VGTx</Device>
           <Vendor>STMicroelectronics</Vendor>
-          <PackID>Keil.STM32L4xx_DFP.2.6.2</PackID>
+          <PackID>Keil.STM32L4xx_DFP.2.1.0</PackID>
-          <PackURL>https://www.keil.com/pack/</PackURL>
+          <PackURL>http://www.keil.com/pack</PackURL>
           <Cpu>IRAM(0x20000000,0x00018000) IRAM2(0x10000000,0x00008000) IROM(0x08000000,0x00100000) CPUTYPE("Cortex-M4") FPU2 DSP CLOCK(12000000) ELITTLE</Cpu>
           <FlashUtilSpec></FlashUtilSpec>
           <StartupFile></StartupFile>
@@ -184,8 +184,6 @@
             <hadXRAM>0</hadXRAM>
             <uocXRam>0</uocXRam>
             <RvdsVP>1</RvdsVP>
-            <RvdsMve>0</RvdsMve>
-            <RvdsCdeCp>0</RvdsCdeCp>
             <hadIRAM2>1</hadIRAM2>
             <hadIROM2>0</hadIROM2>
             <StupSel>8</StupSel>
@@ -352,7 +350,7 @@
             <NoWarn>0</NoWarn>
             <uSurpInc>0</uSurpInc>
             <useXO>0</useXO>
-            <ClangAsOpt>1</ClangAsOpt>
+            <uClangAs>0</uClangAs>
             <VariousControls>
               <MiscControls></MiscControls>
               <Define></Define>
@@ -1257,7 +1255,7 @@
                 <NoWarn>2</NoWarn>
                 <uSurpInc>2</uSurpInc>
                 <useXO>2</useXO>
-                <ClangAsOpt>0</ClangAsOpt>
+                <uClangAs>2</uClangAs>
                 <VariousControls>
                   <MiscControls></MiscControls>
                   <Define></Define>
@@ -1341,7 +1339,7 @@
                 <NoWarn>2</NoWarn>
                 <uSurpInc>2</uSurpInc>
                 <useXO>2</useXO>
-                <ClangAsOpt>1</ClangAsOpt>
+                <uClangAs>2</uClangAs>
                 <VariousControls>
                   <MiscControls></MiscControls>
                   <Define></Define>
@@ -1398,13 +1396,4 @@
     <files/>
   </RTE>
 
-  <LayerInfo>
-    <Layers>
-      <Layer>
-        <LayName>controller-v2</LayName>
-        <LayPrjMark>1</LayPrjMark>
-      </Layer>
-    </Layers>
-  </LayerInfo>
-
 </Project>

User/application/mode/mode_pwmp_hd.c

@@ -23,6 +23,7 @@ pwmp_hd_adjust_t *mode_pwmp_hd_adjust;
 pid_autotune_hd_t *pid_autotune_hd;
 pwmp_hd_test_t *mode_pwmp_hd_test;
 execute_rsp_hd_t rsp; // 执行结果
+mode_pwmp_hd_valve_type_e valve_type_hd = ANGULAR_TRAVEL;
 
 static execute_rsp_hd_t *execute_dac_plan1(void); // 执行方案1
 static execute_rsp_hd_t *execute_dac_plan2(void); // 执行方案2
@@ -177,14 +178,28 @@ static float32 get_pwmp_hd_control_kp(void)
 
 static float32 get_pwmp_hd_control_ki(void)
 {
-    return mode_pwmp_hd->pwmp_save->storage.ki < 0.001 ? mode_pwmp_hd->pwmp_save->storage.ki * 10 : 0.01;
+    return mode_pwmp_hd->pwmp_save->storage.ki < 0.001 ? mode_pwmp_hd->pwmp_save->storage.ki * 20 : 0.01;
-    // return mode_pwmp_hd->pwmp_save->storage.ki;
 }
 
 static float32 get_pwmp_hd_control_kd(void)
 {
     // return mode_pwmp_hd->pwmp_save->storage.kd < 0.01 ? mode_pwmp_hd->pwmp_save->storage.kd : 0.001;
-    return mode_pwmp_hd->pwmp_save->storage.kd;
+    if (valve_type_hd == LINEAR_TRAVEL)
+    {
+        return mode_pwmp_hd->pwmp_save->storage.kd = 0;
+    }
+    else
+    {
+
+        if (mode_pwmp_hd->pwmp_save->storage.kd <= 20)
+        {
+            return mode_pwmp_hd->pwmp_save->storage.kd;
+        }
+        else
+        {
+            return mode_pwmp_hd->pwmp_save->storage.kd = 20;
+        }
+    }
 }
 
 /*将自己结构体变量中的参数保存到公共参数中*/
@@ -267,8 +282,6 @@ execute_rsp_hd_t *execute_dac(execute_plan_hd_e plan)
 }
 static execute_rsp_hd_t *execute_dac_plan1()
 {
-    static float32 index_min = 0.4; // 加权系数
-    static float32 index_max = 0.8;
     static float32 Control_diff;
     // uint8_t flag_stable = 0;
     Control_diff = mode_pwmp_hd->pwmp_save->storage.pwmp_max_origin - mode_pwmp_hd->pwmp_save->storage.pwmp_min_origin;
@@ -276,91 +289,65 @@ static execute_rsp_hd_t *execute_dac_plan1()
 
     // 杭电：输出限幅的参数设置
     _pid.pid_u.hd.set_out_prm_position(&_pid.pid_u.hd, 100, 0);
-
     out = _pid.pid_u.hd.pid_position(&_pid.pid_u.hd, mode_pwmp_hd->control.real_error);
-    out = out / 100;                                                                                                                                                      // 对PID输出结果进行归一化处理
+    out = out / 100;
-    out *= (mode_pwmp_hd->pwmp_save->storage.pwmp_max_origin + Control_diff * index_max) - (mode_pwmp_hd->pwmp_save->storage.pwmp_min_origin - Control_diff * index_min); // 映射实际控制器的范围
+    if (valve_type_hd == LINEAR_TRAVEL)
-    out += mode_pwmp_hd->pwmp_save->storage.pwmp_min_origin - Control_diff * index_min;                                                                                   // 对输出结果进行偏移处理
+    {
+        /* 一阶段加权系数*/
+        static float32 index_min_first = 0.5;
+        static float32 index_max_first = 0.5;
+        /*二阶段加权系数*/
+        static float32 index_min_second = 0.3;
+        static float32 index_max_second = 0.3;
+        if (fabs(mode_pwmp_hd->control.real_error) >= 5.7)
+        {                                                                                                                                                                                     // 对PID输出结果进行归一化处理
+            out *= (mode_pwmp_hd->pwmp_save->storage.pwmp_max_origin + Control_diff * index_max_first) - (mode_pwmp_hd->pwmp_save->storage.pwmp_min_origin - Control_diff * index_min_first); // 映射实际控制器的范围
+            out += mode_pwmp_hd->pwmp_save->storage.pwmp_min_origin - Control_diff * index_min_first;
+        }
+        else if (fabs(mode_pwmp_hd->control.real_error) < 5.7 && fabs(mode_pwmp_hd->control.real_error) >= 1)
+        {                                                                                                                                                                                       // 对PID输出结果进行归一化处理
+            out *= (mode_pwmp_hd->pwmp_save->storage.pwmp_max_origin + Control_diff * index_max_second) - (mode_pwmp_hd->pwmp_save->storage.pwmp_min_origin - Control_diff * index_min_second); // 映射实际控制器的范围
+            out += mode_pwmp_hd->pwmp_save->storage.pwmp_min_origin - Control_diff * index_min_second;
+        }
+        else
+        {
+            out *= (mode_pwmp_hd->pwmp_save->storage.pwmp_max_origin) - (mode_pwmp_hd->pwmp_save->storage.pwmp_min_origin); // 映射实际控制器的范围
+            out += mode_pwmp_hd->pwmp_save->storage.pwmp_min_origin;
+        }
+    }
+    else if (valve_type_hd == ANGULAR_TRAVEL)
+    {
+        /* 一阶段加权系数*/
+        static float32 index_min_first = 1.2;
+        static float32 index_max_first = 1.2;
+        /*二阶段加权系数*/
+        static float32 index_min_second = 0.8;
+        static float32 index_max_second = 0.8;
+        /*三阶段加权系数*/
+        static float32 index_min_third = 0;
+        static float32 index_max_third = 0;
+        if (fabs(mode_pwmp_hd->control.real_error) >= 5.7)
+        {                                                                                                                                                                                     // 对PID输出结果进行归一化处理
+            out *= (mode_pwmp_hd->pwmp_save->storage.pwmp_max_origin + Control_diff * index_max_first) - (mode_pwmp_hd->pwmp_save->storage.pwmp_min_origin - Control_diff * index_min_first); // 映射实际控制器的范围
+            out += mode_pwmp_hd->pwmp_save->storage.pwmp_min_origin - Control_diff * index_min_first;
+        }
+        else if (fabs(mode_pwmp_hd->control.real_error) < 5.7 && fabs(mode_pwmp_hd->control.real_error) >= 1)
+        {                                                                                                                                                                                       // 对PID输出结果进行归一化处理
+            out *= (mode_pwmp_hd->pwmp_save->storage.pwmp_max_origin + Control_diff * index_max_second) - (mode_pwmp_hd->pwmp_save->storage.pwmp_min_origin - Control_diff * index_min_second); // 映射实际控制器的范围
+            out += mode_pwmp_hd->pwmp_save->storage.pwmp_min_origin - Control_diff * index_min_second;
+        }
+        else
+        {
+            out *= (mode_pwmp_hd->pwmp_save->storage.pwmp_max_origin - Control_diff * index_max_third) - (mode_pwmp_hd->pwmp_save->storage.pwmp_min_origin + Control_diff * index_min_third); // 映射实际控制器的范围
+            out += (mode_pwmp_hd->pwmp_save->storage.pwmp_min_origin + Control_diff * index_min_third);
+        }
+    }
 
     rsp.code = EXECUTE_HD_OUT;
     rsp.dac = (uint32_t)out;
 
-    /*
-        /// 在执行了十次之后若系统认为稳定进入以下程序进行微调(非线性补偿器)
-        if (execute_run_count >= 10)
-        {
-            if ( rt_data.actual_travel < target_travel)
-            {
-                if (fabs(mode_pwmp_hd->control.real_error) <= 0.5)
-                {
-                    if (flag_stable == 1)
-                    {
-                        rsp.dac -= 0;
-                        flag_stable = 0;
-                    }
-                    execute_run_count = 0;
-                }
-                else
-                {
-                    if (fabs(mode_pwmp_hd->control.real_error) < 1)
-                    {
-                        // rsp.dac += (fabs(mode_pwmp_hd->control.real_error) * 40);
-                        rsp.dac += (fabs(mode_pwmp_hd->control.real_error) * 0);
-                        flag_stable = 1;
-                    }
-                    else if (fabs(mode_pwmp_hd->control.real_error) < 5)
-                    {
-                        // rsp.dac += (fabs(mode_pwmp_hd->control.real_error) * 30);
-                        rsp.dac += (fabs(mode_pwmp_hd->control.real_error) * 0);
-                    }
-                    else
-                    {
-                        execute_run_count = 0;
-                    }
-                }
-            }
-            if (target_travel <=  rt_data.actual_travel)
-            {
-                // 若进入死区范围，那么停止计数，停止微调
-                if (fabs(mode_pwmp_hd->control.real_error) <= 0.5)
-                {
-                    if (flag_stable == 1)
-                    {
-                        rsp.dac += 0;
-                        flag_stable = 0;
-                    }
-                    execute_run_count = 0;
-                }
-                else
-                {
-                    if (fabs(mode_pwmp_hd->control.real_error) < 1)
-                    {
-                        rsp.dac += -(fabs(mode_pwmp_hd->control.real_error) * 0);
-                        // rsp.dac += -(fabs(mode_pwmp_hd->control.real_error) * 40);
-                        flag_stable = 1;
-                    }
-                    else if (fabs(mode_pwmp_hd->control.real_error) < 5)
-                    {
-                        // rsp.dac += -(fabs(mode_pwmp_hd->control.real_error) * 30);
-                        rsp.dac += (fabs(mode_pwmp_hd->control.real_error) * 0);
-                    }
-                    else
-                    {
-                        execute_run_count = 0;
-                    }
-                    // execute_run_count = 2;
-                }
-            }
-        }
-        else
-        {
-            execute_run_count++;
-        }
-    */
-
     return &rsp;
 }
-
 /// <summary>
 ///
 /// </summary>
@@ -777,7 +764,6 @@ static void pwmp_adjust_hd_rough_position100(uint8_t *state, mode_pwmp_hd_adjust
             return;
         }
 
-        sys_millis_reset();
         mode_pwmp_hd_adjust->all_open_time_full = sys_millis(); // 记录全开起始时间
         mode_pwmp_hd_adjust->tmp_time = 0;
 
@@ -906,7 +892,6 @@ static void pwmp_adjust_hd_all_close_time(uint8_t *state, mode_pwmp_hd_adjust_st
         mode_pwmp_hd_adjust->arr_current = mode_pwmp_hd_adjust->arr_record_0 - 100; // 此处的输入信号大小根据实际控制的最小值来设定
 
         /*记录起始的全开时间*/
-        sys_millis_reset();
         mode_pwmp_hd_adjust->all_close_time = sys_millis();
         mode_pwmp_hd_adjust->tmp_time = 0;
 
@@ -941,7 +926,6 @@ static void pwmp_adjust_hd_all_open_time(uint8_t *state, mode_pwmp_hd_adjust_sta
         mode_pwmp_hd_adjust->arr_current = mode_pwmp_hd_adjust->arr_record_2 + 100; // 此处的输入信号大小根据实际控制的最大值来设定
 
         /*记录起始的全开时间*/
-        sys_millis_reset();
         mode_pwmp_hd_adjust->all_open_time = sys_millis();
         mode_pwmp_hd_adjust->tmp_time = 0;
 

User/application/mode/mode_pwmp_hd.h

@@ -20,6 +20,11 @@
 
 /*----------------------------------------*/
 typedef enum
+{
+    ANGULAR_TRAVEL,
+    LINEAR_TRAVEL,
+} mode_pwmp_hd_valve_type_e;    // 阀门类型
+typedef enum
 {
     PWMP_HD_PROCESS_CONTROL,
     PWMP_HD_PROCESS_ADJUST,

User/application/src/params.c

@@ -144,7 +144,7 @@ void params_restart(void)
     // 执行机构行程类型：直行程/角行程   TRAVEL_TYPE_STRAIGHT/TRAVEL_TYPE_ANGULAR
     udevice.value_travel_type = TRAVEL_TYPE_STRAIGHT;
     // 执行机构气动类型：气开/气关   ATO/ATC
-    udevice.value_action_type = ATO;
+    udevice.value_action_type = ATC;
     // 执行机构作用类型：单作用/双作用
     udevice.value_relay_type = VALVE_ACTING_SINGLE;
 
@@ -335,6 +335,9 @@ void params_restart(void)
     // 回路电流
     calib_param[CALIBPARA_LOOP].value[0] = 0.611153543;
     calib_param[CALIBPARA_LOOP].value[1] = 2.1390624;
+    // // 回路电流
+    // calib_param[CALIBPARA_LOOP].value[0] = 0.609059751;
+    // calib_param[CALIBPARA_LOOP].value[1] = -0.761328101;
 
     // 4~20ma PWM输出校准
     set_pwm_calib_current();

User/lib/control/custom/pid_hd.c

@@ -3,11 +3,6 @@
 #include "sys.h"
 #include "app.h"
 
-float32 out_pos;  // 位置式pid输出
-float32 Kp_watch; // 观测Kp的大小
-float32 Ki_watch; // 观测Kp的大小
-float32 Kd_watch; // 观测Kp的大小
-
 #if INCOMPLETE_DIFFEREN_HD == 1 // 积分分离
 /*计算微分项，使用追随误差微分项*/
 static float32 td_derivative(struct PID_HD *self, float32 current_err, float32 pre_err, float32 dt)
@@ -27,9 +22,6 @@ static void _set_ctrl_prm_position(struct PID_HD *self, float32 kp, float32 ki,
     osel_memset((uint8_t *)pri, 0, sizeof(pid_hd_position_t));
 
     /*观测传进来的Kp、Ki、Kd*/
-    Kp_watch = kp;
-    Ki_watch = ki;
-    Kd_watch = kd;
 
     pri->kp = kp;
     pri->ki = ki;
@@ -50,12 +42,11 @@ static void _set_out_prm_position(struct PID_HD *self, float32 maximum, float32
     self->pri_u.position.out_min = minimum;
 }
 
-float32 out_pos_watch;
 /*杭电：位置式PID控制算法*/
 static float32 _pid_position(struct PID_HD *self, float32 err)
 {
     /*计算控制的运行时间*/
-    sys_millis_reset();
+    // sys_millis_reset();
     self->pri_u.position.control_time = sys_millis();
     self->pri_u.position.tmp_time = 0;
 
@@ -127,9 +118,7 @@ static float32 _pid_position(struct PID_HD *self, float32 err)
         // self->pri_u.position.out = self->pri_u.position.pre_out;
         x[0] = self->pri_u.position.err;
         x[1] = self->pri_u.position.ki_error;
-        out_pos = self->pri_u.position.kp * x[0] + self->pri_u.position.ki * x[1] + self->pri_u.position.kd * x[2];
+        self->pri_u.position.out = self->pri_u.position.kp * x[0] + self->pri_u.position.ki * x[1] + self->pri_u.position.kd * x[2];
-        out_pos_watch = out_pos;
-        self->pri_u.position.out = out_pos;
     }
     else
     {
@@ -147,9 +136,7 @@ static float32 _pid_position(struct PID_HD *self, float32 err)
         x[2] = self->pri_u.position.err - self->pri_u.position.pre_error;
 #endif
 
-        out_pos = self->pri_u.position.kp * x[0] + self->pri_u.position.ki * x[1] + self->pri_u.position.kd * x[2];
+        self->pri_u.position.out = self->pri_u.position.kp * x[0] + self->pri_u.position.ki * x[1] + self->pri_u.position.kd * x[2];
-        out_pos_watch = out_pos;
-        self->pri_u.position.out = out_pos;
     }
 
     /*输出限幅*/
@@ -167,7 +154,6 @@ static float32 _pid_position(struct PID_HD *self, float32 err)
     // 更新输出历史
     self->pri_u.position.pre_out = self->pri_u.position.out; /*上一次输出值*/
 
-    out_pos = self->pri_u.position.out;
     return self->pri_u.position.out;
 }