2026年3月24日，兴先达设备调试版本，校准算法初版

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

@@ -49,7 +49,11 @@ bool count_smaller(int32_t *arr, uint8_t size, int32_t target);
 uint16_t CRC_Modbus(uint16_t wBase, uint8_t *para, uint16_t length);
 uint8_t CRC_Sum(uint8_t *_pbuff, uint16_t _cmdLen);
 bool isAllZero(uint8_t *arr, int size);
-int fixWithMedianWindow(int arr[], int size, int windowSize, int threshold);
+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 enhanceStart, int enhanceEnd, 
+                                      double enhanceFactor, int verbose);
 #endif
 
 

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

@@ -204,12 +204,14 @@
 #define	TWIN_BIT0_PINx							(Pin12)
 #define	SET_TWIN_BIT0()						PORT_SetBits(TWIN_BIT0_PORTx, TWIN_BIT0_PINx)//拉高
 #define	CLR_TWIN_BIT0()						PORT_ResetBits(TWIN_BIT0_PORTx, TWIN_BIT0_PINx)//拉低
+#define	GET_BIT0_DOUT()						PORT_GetBit(TWIN_BIT0_PORTx, TWIN_BIT0_PINx)
 
 //<		BIT1
 #define	TWIN_BIT1_PORTx						(PortB)
 #define	TWIN_BIT1_PINx							(Pin13)
 #define	SET_TWIN_BIT1()						PORT_SetBits(TWIN_BIT1_PORTx, TWIN_BIT1_PINx)//拉高
 #define	CLR_TWIN_BIT1()						PORT_ResetBits(TWIN_BIT1_PORTx, TWIN_BIT1_PINx)//拉低
+#define	GET_BIT1_DOUT()						PORT_GetBit(TWIN_BIT1_PORTx, TWIN_BIT1_PINx)
 
 typedef struct {
 	int32_t ControlLevel;//控制等级

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

@@ -31,7 +31,7 @@
 #define	BDC_STOP()			(SET_TWIN_BIT0(),SET_TWIN_BIT1())
 #define BDC_UP()				(SET_TWIN_BIT0(),CLR_TWIN_BIT1())
 #define	BDC_DOWN()			(SET_TWIN_BIT1(),CLR_TWIN_BIT0())
-	
+
 //#define	BDC_STOP()				Motor_Control(MOTOR_CMD_STOP)
 //#define BDC_UP()					Motor_Control(MOTOR_CMD_FORWARD)
 //#define	BDC_DOWN()				Motor_Control(MOTOR_CMD_REVERSE)
@@ -67,7 +67,9 @@ typedef struct{
 }ADTrack_t;
 typedef struct{//电机参数
 	bool InitialFlag;//初始标志位
+	bool RestFlag;
 	bool CaliFlag;
+	bool LoopOne;//循环一次标志位
 	int16_t Current_height;//当前高度mm
 	int16_t CaliNum;//标校数量
 	int32_t LU_HeightStress[488];//左上传感器对应高度应力
@@ -101,6 +103,7 @@ typedef enum {
 	APP_STATUS_WAIT_MOTOR_CALI_DOWN,
 	APP_STATUS_WAIT_STEADY,
 	APP_STATUS_ZERO_MARKING,
+	APP_STATUS_HEIGHT_CONTROL,
 }AppStatus_m;
 
 typedef struct{
@@ -134,6 +137,8 @@ typedef struct {
 	uint32_t RunCollDelay1mSCnt;
 	uint32_t EmergencyDelay1mSCnt;
 	uint32_t SlopeCollDelay1mSCnt;
+	uint32_t CailUp1mSCnt;
+	uint32_t HeightContRolCnt;
 	uint8_t FeedDogDlyCnt;
 	
 	bool CaliZeroFlag;//校零标志位