下半区传感器接线方式修改，屏幕安装点过低或过高，按压上半区或下半区屏幕会造成误触发，未能得到解决

Project/HC32F460/MDK/HC32F460.uvoptx

@@ -103,7 +103,7 @@
         <bEvRecOn>1</bEvRecOn>
         <bSchkAxf>0</bSchkAxf>
         <bTchkAxf>0</bTchkAxf>
-        <nTsel>12</nTsel>
+        <nTsel>3</nTsel>
         <sDll></sDll>
         <sDllPa></sDllPa>
         <sDlgDll></sDlgDll>
@@ -228,96 +228,168 @@
         <Ww>
           <count>14</count>
           <WinNumber>1</WinNumber>
-          <ItemText>App.MotorPara.LU_HeightStress,0x0A</ItemText>
+          <ItemText>App.TD1mSDelayCnt,0x0A</ItemText>
         </Ww>
         <Ww>
           <count>15</count>
           <WinNumber>1</WinNumber>
-          <ItemText>App.MotorPara.LL_HeightStress,0x0A</ItemText>
+          <ItemText>App.Status</ItemText>
         </Ww>
         <Ww>
           <count>16</count>
           <WinNumber>1</WinNumber>
-          <ItemText>App.MotorPara.RU_HeightStress,0x0A</ItemText>
+          <ItemText>App.MotorPara.CaliNum,0x0A</ItemText>
         </Ww>
         <Ww>
           <count>17</count>
           <WinNumber>1</WinNumber>
-          <ItemText>App.MotorPara.RL_HeightStress,0x0A</ItemText>
+          <ItemText>App.CailUp1mSCnt,0x0A</ItemText>
         </Ww>
         <Ww>
           <count>18</count>
           <WinNumber>1</WinNumber>
-          <ItemText>App.TD1mSDelayCnt,0x0A</ItemText>
+          <ItemText>App.MotorPara.RestFlag,0x0A</ItemText>
         </Ww>
         <Ww>
           <count>19</count>
           <WinNumber>1</WinNumber>
-          <ItemText>App.RxUart3Data.RxBuff,0x10</ItemText>
+          <ItemText>App.MotorPara.Current_height,0x0A</ItemText>
         </Ww>
         <Ww>
           <count>20</count>
           <WinNumber>1</WinNumber>
-          <ItemText>App.RxUart3Data.RxLen,0x0A</ItemText>
+          <ItemText>App.HeightContRolCnt,0x0A</ItemText>
         </Ww>
         <Ww>
           <count>21</count>
           <WinNumber>1</WinNumber>
-          <ItemText>App.Status</ItemText>
+          <ItemText>App.aaa,0x0A</ItemText>
         </Ww>
         <Ww>
           <count>22</count>
           <WinNumber>1</WinNumber>
-          <ItemText>App.MotorPara.CaliNum,0x0A</ItemText>
+          <ItemText>App.bbb,0x0A</ItemText>
         </Ww>
         <Ww>
           <count>23</count>
           <WinNumber>1</WinNumber>
-          <ItemText>App.CailUp1mSCnt,0x0A</ItemText>
+          <ItemText>App.ccc,0x0A</ItemText>
         </Ww>
         <Ww>
           <count>24</count>
           <WinNumber>1</WinNumber>
-          <ItemText>Oru_diff1,0x0A</ItemText>
+          <ItemText>App.ddd,0x0A</ItemText>
         </Ww>
         <Ww>
           <count>25</count>
           <WinNumber>1</WinNumber>
-          <ItemText>Orl_diff1,0x0A</ItemText>
+          <ItemText>App.ADTrack.LeftUpVPT,0x0A</ItemText>
         </Ww>
         <Ww>
           <count>26</count>
           <WinNumber>1</WinNumber>
-          <ItemText>App.ADTrack.Stop_VPT,0x0A</ItemText>
+          <ItemText>App.ADTrack.LeftLowVPT,0x0A</ItemText>
         </Ww>
         <Ww>
           <count>27</count>
           <WinNumber>1</WinNumber>
-          <ItemText>ru_ad[0],0x0A</ItemText>
+          <ItemText>App.ADTrack.RightUpVPT,0x0A</ItemText>
         </Ww>
         <Ww>
           <count>28</count>
           <WinNumber>1</WinNumber>
-          <ItemText>App.MotorPara.RestFlag,0x0A</ItemText>
+          <ItemText>App.ADTrack.RightLowVPT,0x0A</ItemText>
         </Ww>
         <Ww>
           <count>29</count>
           <WinNumber>1</WinNumber>
-          <ItemText>App.MotorPara.Current_height,0x0A</ItemText>
+          <ItemText>App.ADTrack.Stop_VPT,0x0A</ItemText>
         </Ww>
         <Ww>
           <count>30</count>
           <WinNumber>1</WinNumber>
-          <ItemText>App.HeightContRolCnt,0x0A</ItemText>
+          <ItemText>App.MotorPara.Current_height,0x0A</ItemText>
         </Ww>
       </WatchWindow1>
+      <WatchWindow2>
+        <Ww>
+          <count>0</count>
+          <WinNumber>2</WinNumber>
+          <ItemText>Olu_diff1,0x0A</ItemText>
+        </Ww>
+        <Ww>
+          <count>1</count>
+          <WinNumber>2</WinNumber>
+          <ItemText>Oll_diff1,0x0A</ItemText>
+        </Ww>
+        <Ww>
+          <count>2</count>
+          <WinNumber>2</WinNumber>
+          <ItemText>Oru_diff1,0x0A</ItemText>
+        </Ww>
+        <Ww>
+          <count>3</count>
+          <WinNumber>2</WinNumber>
+          <ItemText>Orl_diff1,0x0A</ItemText>
+        </Ww>
+        <Ww>
+          <count>4</count>
+          <WinNumber>2</WinNumber>
+          <ItemText>Olu_diff2,0x0A</ItemText>
+        </Ww>
+        <Ww>
+          <count>5</count>
+          <WinNumber>2</WinNumber>
+          <ItemText>Oll_diff2,0x0A</ItemText>
+        </Ww>
+        <Ww>
+          <count>6</count>
+          <WinNumber>2</WinNumber>
+          <ItemText>Oru_diff2,0x0A</ItemText>
+        </Ww>
+        <Ww>
+          <count>7</count>
+          <WinNumber>2</WinNumber>
+          <ItemText>Orl_diff2,0x0A</ItemText>
+        </Ww>
+        <Ww>
+          <count>8</count>
+          <WinNumber>2</WinNumber>
+          <ItemText>Olu_diff3,0x0A</ItemText>
+        </Ww>
+        <Ww>
+          <count>9</count>
+          <WinNumber>2</WinNumber>
+          <ItemText>Oll_diff3,0x0A</ItemText>
+        </Ww>
+        <Ww>
+          <count>10</count>
+          <WinNumber>2</WinNumber>
+          <ItemText>Oru_diff3,0x0A</ItemText>
+        </Ww>
+        <Ww>
+          <count>11</count>
+          <WinNumber>2</WinNumber>
+          <ItemText>Orl_diff3,0x0A</ItemText>
+        </Ww>
+        <Ww>
+          <count>12</count>
+          <WinNumber>2</WinNumber>
+          <ItemText>LeftDiff,0x0A</ItemText>
+        </Ww>
+        <Ww>
+          <count>13</count>
+          <WinNumber>2</WinNumber>
+          <ItemText>RightDiff,0x0A</ItemText>
+        </Ww>
+      </WatchWindow2>
       <Tracepoint>
         <THDelay>0</THDelay>
       </Tracepoint>
       <DebugFlag>
         <trace>0</trace>
         <periodic>1</periodic>
-        <aLwin>1</aLwin>
+        <aLwin>0</aLwin>
         <aCover>0</aCover>
         <aSer1>0</aSer1>
         <aSer2>0</aSer2>
@@ -1192,6 +1264,26 @@
     </File>
   </Group>
 
+  <Group>
+    <GroupName>BeepAndLed</GroupName>
+    <tvExp>1</tvExp>
+    <tvExpOptDlg>0</tvExpOptDlg>
+    <cbSel>0</cbSel>
+    <RteFlg>0</RteFlg>
+    <File>
+      <GroupNumber>6</GroupNumber>
+      <FileNumber>55</FileNumber>
+      <FileType>1</FileType>
+      <tvExp>0</tvExp>
+      <tvExpOptDlg>0</tvExpOptDlg>
+      <bDave2>0</bDave2>
+      <PathWithFileName>..\source\Module\BeepAndLed\BeepAndLed.c</PathWithFileName>
+      <FilenameWithoutPath>BeepAndLed.c</FilenameWithoutPath>
+      <RteFlg>0</RteFlg>
+      <bShared>0</bShared>
+    </File>
+  </Group>
+
   <Group>
     <GroupName>Readme</GroupName>
     <tvExp>0</tvExp>

Project/HC32F460/MDK/HC32F460.uvprojx

@@ -10,14 +10,14 @@
       <TargetName>Debug</TargetName>
       <ToolsetNumber>0x4</ToolsetNumber>
       <ToolsetName>ARM-ADS</ToolsetName>
-      <pArmCC>5060960::V5.06 update 7 (build 960)::ARMCC</pArmCC>
+      <pArmCC>5060960::V5.06 update 7 (build 960)::.\ARMCC</pArmCC>
-      <pCCUsed>5060960::V5.06 update 7 (build 960)::ARMCC</pCCUsed>
+      <pCCUsed>5060960::V5.06 update 7 (build 960)::.\ARMCC</pCCUsed>
       <uAC6>0</uAC6>
       <TargetOption>
         <TargetCommonOption>
           <Device>HC32F460JETA</Device>
           <Vendor>HDSC</Vendor>
-          <PackID>HDSC.HC32F460.1.0.12</PackID>
+          <PackID>HDSC.HC32F460.1.0.11</PackID>
           <PackURL>https://raw.githubusercontent.com/hdscmcu/pack/master/</PackURL>
           <Cpu>IRAM(0x1FFF8000,0x2F000) IRAM2(0x200F0000,0x1000) IROM(0x00000000,0x80000) IROM2(0x03000C00,0x003FC) CPUTYPE("Cortex-M4") FPU2 CLOCK(12000000) ELITTLE</Cpu>
           <FlashUtilSpec></FlashUtilSpec>
@@ -341,7 +341,7 @@
               <MiscControls></MiscControls>
               <Define>__DEBUG,HC32F460,USE_DEVICE_DRIVER_LIB,JEUA</Define>
               <Undefine></Undefine>
-              <IncludePath>..\..\..\mcu\common;..\source\User\Inc;..\source\User\Src;..\..\..\driver\src;..\..\..\driver\inc;..\source\Module\CS1237;..\source\Module\dc_motor</IncludePath>
+              <IncludePath>..\..\..\mcu\common;..\source\User\Inc;..\source\User\Src;..\..\..\driver\src;..\..\..\driver\inc;..\source\Module\CS1237;..\source\Module\dc_motor;..\source\Module\BeepAndLed</IncludePath>
             </VariousControls>
           </Cads>
           <Aads>
@@ -677,6 +677,16 @@
             </File>
           </Files>
         </Group>
+        <Group>
+          <GroupName>BeepAndLed</GroupName>
+          <Files>
+            <File>
+              <FileName>BeepAndLed.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\source\Module\BeepAndLed\BeepAndLed.c</FilePath>
+            </File>
+          </Files>
+        </Group>
         <Group>
           <GroupName>Readme</GroupName>
         </Group>
@@ -695,7 +705,7 @@
         <TargetCommonOption>
           <Device>HC32F460PETB</Device>
           <Vendor>HDSC</Vendor>
-          <PackID>HDSC.HC32F460.1.0.12</PackID>
+          <PackID>HDSC.HC32F460.1.0.11</PackID>
           <PackURL>https://raw.githubusercontent.com/hdscmcu/pack/master/</PackURL>
           <Cpu>IROM(0x00000000,0x80000) IROM2(0x03000C00,0x3FC) IRAM2(0x200F0000,0x1000) IROM(0x00000000,0x80000) CPUTYPE("Cortex-M4") FPU2 CLOCK(12000000) ELITTLE</Cpu>
           <FlashUtilSpec></FlashUtilSpec>
@@ -1355,6 +1365,16 @@
             </File>
           </Files>
         </Group>
+        <Group>
+          <GroupName>BeepAndLed</GroupName>
+          <Files>
+            <File>
+              <FileName>BeepAndLed.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\source\Module\BeepAndLed\BeepAndLed.c</FilePath>
+            </File>
+          </Files>
+        </Group>
         <Group>
           <GroupName>Readme</GroupName>
         </Group>

Project/HC32F460/MDK/startup_hc32f460.s

@@ -224,7 +224,7 @@ Reset_Handler   PROC
                 IMPORT  SystemInit
                 IMPORT  __main
 SET_SRAM3_WAIT
-                LDR     R0, =0x40050804
+1                LDR     R0, =0x40050804
                 MOV     R1, #0x77
                 STR     R1, [R0]
 

Project/HC32F460/source/Module/BeepAndLed/BeepAndLed.c

@@ -0,0 +1,177 @@
+#include "BeepAndLed.h"
+#include "beepAndLed.h"
+#include "main.h"  // 包含BUZZER_ON/OFF, LED等宏定义
+
+// ========== 变量定义 ==========
+static BeeperCtrl_t g_beeper = {
+    .state = BEEPER_STATE_IDLE,
+    .is_active = 0,
+    .repeat = 0,
+    .repeat_done = 0,
+    .on_ms = 0,
+    .off_ms = 0
+};
+
+// 延时计数器（供滴答定时器使用）
+uint32_t BeepAndLedCollDelay1mSCnt = 0;
+
+// ========== 硬件控制函数（根据您的实际宏定义修改）==========
+static void Beeper_HW_On(void)
+{
+    BUZZER_ON();      // 请替换为您的实际宏
+    LED_GREEN_OFF();  // 请替换为您的实际宏
+    LED_RED_ON();     // 请替换为您的实际宏
+}
+
+static void Beeper_HW_Off(void)
+{
+    BUZZER_OFF();     // 请替换为您的实际宏
+    LED_GREEN_ON();   // 请替换为您的实际宏
+    LED_RED_OFF();    // 请替换为您的实际宏
+}
+
+// ========== 实现函数 ==========
+
+/**
+ * @brief 蜂鸣器初始化
+ */
+void Beeper_Init(void)
+{
+    Beeper_Stop();
+}
+
+/**
+ * @brief 开始鸣响（非阻塞）
+ * @param on_ms   鸣响时间(ms)
+ * @param off_ms  间隔时间(ms)
+ * @param repeat  重复次数（0表示无效）
+ * 
+ * @example Beeper_Start(200, 100, 3);  // 鸣200ms，停100ms，重复3次
+ */
+void Beeper_Start(uint16_t on_ms, uint16_t off_ms, uint8_t repeat)
+{
+    if (repeat == 0) return;
+    
+    // 如果已经在鸣响，可以选择停止并重新开始
+    if (g_beeper.is_active) {
+        Beeper_Stop();  // 停止当前，重新开始新的
+        // return;      // 或者选择忽略新请求
+    }
+    
+    // 保存参数
+    g_beeper.on_ms = on_ms;
+    g_beeper.off_ms = off_ms;
+    g_beeper.repeat = repeat;
+    g_beeper.repeat_done = 0;
+    g_beeper.is_active = 1;
+    
+    // 开启蜂鸣器
+    Beeper_HW_On();
+    
+    // 设置延时计数器（开启状态延时）
+    BeepAndLedCollDelay1mSCnt = on_ms;
+    g_beeper.state = BEEPER_STATE_ON;
+}
+
+/**
+ * @brief 停止鸣响
+ */
+void Beeper_Stop(void)
+{
+    // 关闭硬件
+    Beeper_HW_Off();
+    
+    // 重置状态机
+    g_beeper.state = BEEPER_STATE_IDLE;
+    g_beeper.is_active = 0;
+    g_beeper.repeat = 0;
+    g_beeper.repeat_done = 0;
+    BeepAndLedCollDelay1mSCnt = 0;
+}
+
+/**
+ * @brief 蜂鸣器状态机更新（放在main.c的while(1)中循环调用）
+ * @note 必须在滴答定时器更新BeepAndLedCollDelay1mSCnt后才能正确工作
+ */
+void BeeperLoopHandler(void)
+{
+    if (!g_beeper.is_active) {
+        return;
+    }
+    
+    switch (g_beeper.state) {
+        case BEEPER_STATE_ON:
+            // 蜂鸣器开启状态，等待计数器归零
+            if (BeepAndLedCollDelay1mSCnt == 0) {
+                // 延时结束，关闭蜂鸣器
+                Beeper_HW_Off();
+                
+                g_beeper.repeat_done++;
+                
+                // 检查是否完成所有重复
+                if (g_beeper.repeat_done >= g_beeper.repeat) {
+                    // 全部完成，结束
+                    g_beeper.is_active = 0;
+                    g_beeper.state = BEEPER_STATE_IDLE;
+                } else {
+                    // 还有下次，进入等待状态
+                    BeepAndLedCollDelay1mSCnt = g_beeper.off_ms;
+                    g_beeper.state = BEEPER_STATE_OFF;
+                }
+            }
+            break;
+            
+        case BEEPER_STATE_OFF:
+            // 蜂鸣器关闭等待状态（间隔时间）
+            if (BeepAndLedCollDelay1mSCnt == 0) {
+                // 间隔结束，开启下一次鸣响
+                Beeper_HW_On();
+                
+                BeepAndLedCollDelay1mSCnt = g_beeper.on_ms;
+                g_beeper.state = BEEPER_STATE_ON;
+            }
+            break;
+            
+        case BEEPER_STATE_WAIT:
+        case BEEPER_STATE_IDLE:
+        default:
+            break;
+    }
+}
+
+/**
+ * @brief 查询是否正在鸣响
+ * @return true:正在鸣响, false:空闲
+ */
+bool Beeper_IsPlaying(void)
+{
+    return g_beeper.is_active;
+}
+
+/**
+ * @brief 手动开启蜂鸣器（不计入状态机）
+ */
+void Beeper_On(void)
+{
+    Beeper_HW_On();
+}
+
+/**
+ * @brief 手动关闭蜂鸣器（不计入状态机）
+ */
+void Beeper_Off(void)
+{
+    Beeper_HW_Off();
+}
+
+/**
+ * @brief 清除延时计数器（由滴答定时器调用）
+ * @note 这个函数放在滴答定时器的1ms中断中调用
+ */
+void Beeper_ClearDelay(void)
+{
+    if (BeepAndLedCollDelay1mSCnt > 0) {
+        BeepAndLedCollDelay1mSCnt--;
+    }
+}
+

Project/HC32F460/source/Module/BeepAndLed/BeepAndLed.h

@@ -0,0 +1,39 @@
+#ifndef __BEEPANDLED_H
+#define __BEEPANDLED_H
+
+#include <stdint.h>
+#include <stdbool.h>
+#include "bsp.h"
+
+// ========== 蜂鸣器状态机状态定义 ==========
+typedef enum {
+    BEEPER_STATE_IDLE = 0,      // 空闲
+    BEEPER_STATE_ON,            // 蜂鸣器开启延时中
+    BEEPER_STATE_OFF,           // 蜂鸣器关闭延时中
+    BEEPER_STATE_WAIT           // 等待下一次鸣响
+} BeeperState_t;
+
+// ========== 蜂鸣器控制结构体 ==========
+typedef struct {
+    BeeperState_t state;        // 当前状态
+    uint16_t on_ms;             // 鸣响时间(ms)
+    uint16_t off_ms;            // 间隔时间(ms)
+    uint8_t repeat;             // 重复次数
+    uint8_t repeat_done;        // 已完成次数
+    uint8_t is_active;          // 是否激活
+} BeeperCtrl_t;
+
+// ========== 外部可访问变量 ==========
+extern uint32_t BeepAndLedCollDelay1mSCnt;  // 延时计数器（在beepAndLed.c中定义）
+
+// ========== API函数 ==========
+void Beeper_Init(void);                          // 初始化
+void Beeper_Start(uint16_t on_ms, uint16_t off_ms, uint8_t repeat);  // 开始鸣响（非阻塞）
+void Beeper_Stop(void);                          // 停止鸣响
+void BeeperLoopHandler(void);                        // 状态机更新（放在while(1)中）
+bool Beeper_IsPlaying(void);                     // 查询是否正在鸣响
+void Beeper_On(void);                            // 手动开启蜂鸣器
+void Beeper_Off(void);                           // 手动关闭蜂鸣器
+void Beeper_ClearDelay(void);                    // 清除延时计数器（供滴答定时器使用）
+#endif /* _BEEP_AND_LED_H_ */
+

Project/HC32F460/source/User/Inc/Algorithm.h

@@ -51,9 +51,8 @@ uint8_t CRC_Sum(uint8_t *_pbuff, uint16_t _cmdLen);
 bool isAllZero(uint8_t *arr, int size);
 int fixWithMedianIterative(int arr[], int size, int windowSize, int threshold, int maxIterations, int verbose);
 int finalCheck(int arr[], int size, int threshold, int verbose);
-int weightedMovingAverageWithEnhance(int32_t arr[], int size, int windowSize,
+int weightedMovingAverageWithEnhance(int32_t arr[], int size, int windowSize,int enhanceStart, int enhanceEnd, double enhanceFactor, int verbose);
-                                      int enhanceStart, int enhanceEnd, 
+int CompareAndCountAbs(const int32_t *arr1, const int32_t *arr2, uint16_t length,int32_t zero1, int32_t zero2, int32_t offset);
-                                      double enhanceFactor, int verbose);
 #endif
 
 

Project/HC32F460/source/User/Inc/bsp.h

@@ -213,6 +213,24 @@
 #define	CLR_TWIN_BIT1()						PORT_ResetBits(TWIN_BIT1_PORTx, TWIN_BIT1_PINx)//拉低
 #define	GET_BIT1_DOUT()						PORT_GetBit(TWIN_BIT1_PORTx, TWIN_BIT1_PINx)
 
+//<		BUZZER
+#define	BUZZER_PORTx							(PortB)
+#define	BUZZER_PINx								(Pin15)
+#define	BUZZER_ON()								PORT_SetBits(BUZZER_PORTx, BUZZER_PINx)//拉高
+#define	BUZZER_OFF()							PORT_ResetBits(BUZZER_PORTx, BUZZER_PINx)//拉低
+
+//< 		LED
+#define	LED_GREEN_PORTx						(PortA)
+#define	LED_GREEN_PINx							(Pin08)
+#define	LED_GREEN_ON()							PORT_SetBits(LED_GREEN_PORTx, LED_GREEN_PINx)//拉高
+#define	LED_GREEN_OFF()						PORT_ResetBits(LED_GREEN_PORTx, LED_GREEN_PINx)//拉低
+
+#define	LED_RED_PORTx							(PortA)
+#define	LED_RED_PINx							(Pin09)
+#define	LED_RED_ON()							PORT_SetBits(LED_RED_PORTx, LED_RED_PINx)//拉高
+#define	LED_RED_OFF()							PORT_ResetBits(LED_RED_PORTx, LED_RED_PINx)//拉低
+
+
 typedef struct {
 	int32_t ControlLevel;//控制等级
 	int32_t	DBGWaveOut;//调试波形输出
@@ -289,6 +307,8 @@ void ExtInt04IntEnable(void);
 void ExtInt04IntDisable(void);
 void ExtInt06IntEnable(void);
 void ExtInt06IntDisable(void);
+void Bit0AndBit1_OUT(void);
+void Bit0AndBit1_IN(void);
 #endif
 
 

Project/HC32F460/source/User/Inc/main.h

@@ -11,26 +11,31 @@
 #define 	SLOPE_CNT			  20
 
 #define 	SLOPE_VPT				1000
-#define		PRESS_VPT				3
+#define		PRESS_VPT				10
 #define		FOLLOW_VPT			10
-#define		SHIELD_VPT			5000
+#define		SHIELD_VPT			2
 #define		EME_STOP_VPT		4000
-#define		CENTRE_VPT			2000
+
+#define		CENTRE_FRONT_CNT			0
+#define		CENTRE_FRONT_SUM			0
+#define		CENTRE_FRONT_VPT			0
+#define		CENTRE_VPT			0
 
 #define 	STOP_TIME_MS		500
+#define 	CENT_TIME_MS		200
 #define 	RUN_TIME_MS			0
 #define		EME_START_MS		0
 #define		EME_STOP_MS			2000
 #define		SLOPE_START_MS	3000
 
-#define		TRAVEL_TIME_MS	30000
+#define		TRAVEL_TIME_MS	50000
 
 #define		PEAK						1200		//1200mm
 #define		VALLEY					720			//720mm
 
 #define	BDC_STOP()			(SET_TWIN_BIT0(),SET_TWIN_BIT1())
-#define BDC_UP()				(SET_TWIN_BIT0(),CLR_TWIN_BIT1())
+#define BDC_DOWN()				(SET_TWIN_BIT0(),CLR_TWIN_BIT1())
-#define	BDC_DOWN()			(SET_TWIN_BIT1(),CLR_TWIN_BIT0())
+#define	BDC_UP()			(SET_TWIN_BIT1(),CLR_TWIN_BIT0())
 
 //#define	BDC_STOP()				Motor_Control(MOTOR_CMD_STOP)
 //#define BDC_UP()					Motor_Control(MOTOR_CMD_FORWARD)
@@ -55,11 +60,6 @@ typedef struct{
 	int32_t RightUpVPT;//右上设备阈值
 	int32_t RightLowVPT;//右下设备阈值
 	
-	int32_t LeftUpShieldVPT;	//左上屏蔽阈值
-	int32_t LeftLowShieldVPT;	//左下屏蔽阈值
-	int32_t RightUpShieldVPT;	//右上屏蔽阈值
-	int32_t RightLowShieldVPT;	//右下屏蔽阈值
-	
 	int32_t LeftUp_Org_AdZero;//左上设备原始AD零点
 	int32_t LeftLow_Org_AdZero;//左下设备原始AD零点
 	int32_t RightUp_Org_AdZero;//右上设备原始AD零点
@@ -135,6 +135,7 @@ typedef struct {
 	uint32_t Uart3RxTimeOut1mSCnt;
 	uint32_t StopCollDelay1mSCnt;
 	uint32_t RunCollDelay1mSCnt;
+	uint32_t CentCollDelay1mSCnt;
 	uint32_t EmergencyDelay1mSCnt;
 	uint32_t SlopeCollDelay1mSCnt;
 	uint32_t CailUp1mSCnt;
@@ -166,6 +167,11 @@ typedef struct {
 	int32_t RightUpCS1237_SlopeArray[SLOPE_CNT];
 	int32_t RightLowCS1237_SlopeArray[SLOPE_CNT];
 	
+	int32_t aaa;
+	int32_t bbb;
+	int32_t ccc;
+	int32_t ddd;
+	
 	bool WaitFlag;
 }AppDetect_t;
 extern AppDetect_t App;

Project/HC32F460/source/User/Src/Algorithm.c

@@ -4,6 +4,50 @@
 #include "Algorithm.h"
 #include "Debug.h"
 												 
+
+#include <stdint.h>
+#include <stdbool.h>
+#include <stdlib.h>  // 用于 abs() 函数
+
+/**
+ * @brief 比较两个数组对应位置的绝对值差值，统计满足条件的个数
+ * 
+ * 判断条件：|arr1[i] - zero1| + offset > |arr2[i] - zero2|
+ * 
+ * @param arr1          数组1指针
+ * @param arr2          数组2指针
+ * @param length        要比较的数组长度（截取值）
+ * @param zero1         数组1的零点值
+ * @param zero2         数组2的零点值
+ * @param offset        补偿值
+ * @return int          满足条件的个数
+ * 
+ * @note 如果 length 为0，返回0
+ * @note 如果 arr1 或 arr2 为 NULL，返回0
+ */
+int CompareAndCountAbs(const int32_t *arr1, const int32_t *arr2, uint16_t length,
+                        int32_t zero1, int32_t zero2, int32_t offset)
+{
+    // 参数校验
+    if (arr1 == NULL || arr2 == NULL || length == 0) {
+        return 0;
+    }
+    
+    int count = 0;
+    
+    for (uint16_t i = 0; i < length; i++) {
+        // 计算绝对值
+        int32_t abs1 = abs(arr1[i] - zero1);
+        int32_t abs2 = abs(arr2[i] - zero2);
+        
+        // 判断：|arr1[i] - zero1| + offset > |arr2[i] - zero2|
+        if (abs1 + offset > abs2) {
+            count++;
+        }
+    }
+    
+    return count;
+}
 /**
  * 加强特定区域的平滑处理（使用更小的权重衰减，更平滑的效果）
  * 

Project/HC32F460/source/User/Src/bsp.c

@@ -517,13 +517,43 @@ static void GPIO_Config(void)
 	PORT_Init(LEFTLOW_CS1237_SCLK_PORTx, LEFTLOW_CS1237_SCLK_PINx, &stcPortInit);
 	PORT_Init(RIGHTUP_CS1237_SCLK_PORTx, RIGHTUP_CS1237_SCLK_PINx, &stcPortInit);
 	PORT_Init(RIGHTLOW_CS1237_SCLK_PORTx, RIGHTLOW_CS1237_SCLK_PINx, &stcPortInit);
+	PORT_Init(BUZZER_PORTx, BUZZER_PINx, &stcPortInit);
+	PORT_Init(LED_GREEN_PORTx, LED_GREEN_PINx, &stcPortInit);
+	PORT_Init(LED_RED_PORTx, LED_RED_PINx, &stcPortInit);
+	LED_GREEN_OFF();
+	LED_RED_OFF();
 	
+	stcPortInit.enPullUp = Enable;//上拉
+}
+void Bit0AndBit1_OUT(void)
+{
+	stc_port_init_t stcPortInit;
+	MEM_ZERO_STRUCT(stcPortInit);
+	
+	/********************************** GPIO输出 ****************************************/
+		stcPortInit.enPinMode = Pin_Mode_Out;
+	stcPortInit.enPinOType = Pin_OType_Cmos;
+	stcPortInit.enPinDrv = Pin_Drv_H;
+	stcPortInit.enPullUp = Disable;//无上拉
+
 	stcPortInit.enPullUp = Enable;//上拉
 	PORT_Init(TWIN_BIT0_PORTx, TWIN_BIT0_PINx, &stcPortInit);
 	PORT_Init(TWIN_BIT1_PORTx, TWIN_BIT1_PINx, &stcPortInit);
 	SET_TWIN_BIT0();
 	SET_TWIN_BIT1();
 }
+void Bit0AndBit1_IN(void)
+{
+	stc_port_init_t stcPortInit;
+	MEM_ZERO_STRUCT(stcPortInit);
+	
+	/********************************** GPIO输入 ****************************************/
+	stcPortInit.enPinMode = Pin_Mode_In;
+	stcPortInit.enPullUp = Disable;//无上拉
+
+	PORT_Init(TWIN_BIT0_PORTx, TWIN_BIT0_PINx, &stcPortInit);
+	PORT_Init(TWIN_BIT1_PORTx, TWIN_BIT1_PINx, &stcPortInit);
+}
 /*********************************左上传感器
  *******************************************************************************
  ** \brief ExtInt00 callback function
@@ -1429,6 +1459,7 @@ void BSP_Init(void)
 	ExtiCh01_Init();
 	ExtiCh06_Init();
 	ExtiCh04_Init();
+	Bit0AndBit1_OUT();
 	
 	DbgUart_Config(700000);
 #if(USE_USART3 == 1)