修改加速过程为脉冲变化,频率变化符合预期,但仍有少量多发,且高频情况下会有问题

PLSR/PLSR/Core/Inc/tim.h

@@ -48,6 +48,9 @@ extern TIM_HandleTypeDef htim13;
 
 extern TIM_HandleTypeDef htim14;
 
+/* 全局变量声明 */
+extern uint32_t g_last_freq; // 上一次计算的频率，用于PLSR_Calculate_FreqByPosition函数
+
 /* USER CODE BEGIN Private defines */
 // PLSR系统配置参数
 #define PLSR_MAX_SECTIONS     10        // 最大段数
@@ -151,6 +154,7 @@ typedef struct {
     uint8_t current_section_num;    // 当前段号
     uint32_t current_freq;          // 当前频率
     uint32_t target_freq;           // 目标频率
+    uint32_t initial_freq;          // 加减速初始频率，专门用于存储加减速开始时的频率
     int32_t pulse_count;           // 当前脉冲计数
     int32_t prevPulseCount;        // 上阶段目标脉冲
     uint32_t start_freq;            // 起始频率
@@ -243,6 +247,8 @@ void PLSR_SetupThreePartExecution(PLSR_RouteConfig_t* route); //<设置三部分
 void PLSR_Accel_Process(PLSR_RouteConfig_t* route); //<加减速执行函数(新的直线加减速)
 void PLSR_Accel_UpdateRates(PLSR_RouteConfig_t* route); //<更新加减速度
 void PLSR_Accel_SetDefaultParams(PLSR_RouteConfig_t* route, uint32_t accel_time_ms, uint32_t decel_time_ms); //<设置默认加减速参数
+uint32_t PLSR_Calculate_FreqByPosition(PLSR_RouteConfig_t* route, uint8_t is_accel); //<根据当前脉冲位置计算频率
+uint32_t integer_sqrt_64(uint64_t x); //<64位整数开平方函数
 
 // ==================== PLSR等待条件处理函数 ====================
 void PLSR_Wait_StartTimer(PLSR_RouteConfig_t* route);            //<等待条件计数器

PLSR/PLSR/Core/Src/tim.c

@@ -27,6 +27,7 @@ PLSR_RouteConfig_t g_plsr_route;        // 全局PLSR路径控制结构体
 int32_t g_plsr_total_pulse_count = 0;   // 全局累加脉冲计数器(程序运行期间持续累加，支持负数)
 uint8_t g_plsr_ext_event_flag = 0;       // 外部事件标志(0-无事件, 1-事件触发)
 int32_t g_plsr_location = 0;       // 全局位置计数器(用于记录当前执行位置，支持负数)
+uint32_t g_last_freq = 0;          // 上一次计算的频率，用于PLSR_Calculate_FreqByPosition函数
 static uint8_t s_pulse_count_direction = 1;  // 脉冲计数方向(1-递增, 0-递减)，用于替代dir_logic判断计数方向
 uint32_t g_plsr_current_target_freq = 0; // 当前段目标频率频率(Hz)，用于检测是否有频率变化
 static uint8_t s_last_direction = 0xFF; // 初始值设为无效值，确保第一次总是认为有方向变化
@@ -1011,6 +1012,42 @@ uint32_t integer_sqrt(uint32_t x)
     
     return result;
 }
+
+/**
+ * @brief 64位整数开平方函数 (使用二分法)
+ * @param x 需要开平方的64位无符号整数
+ * @return 开平方结果
+ * @note 用于处理大数值的开平方，避免溢出
+ */
+uint32_t integer_sqrt_64(uint64_t x)
+{
+    if (x == 0) return 0;
+    if (x == 1) return 1;
+    
+    // 对于64位整数，最大平方根不会超过2^32
+    uint64_t left = 0;
+    uint64_t right = (x > 0xFFFFFFFF) ? 0xFFFFFFFF : x;
+    uint64_t result = 0;
+    
+    while (left <= right) {
+        uint64_t mid = left + (right - left) / 2;
+        
+        // 防止溢出：检查 mid * mid
+        if (mid <= x / mid) {  // 等价于 mid * mid <= x，但避免溢出
+            result = mid;
+            left = mid + 1;
+        } else {
+            right = mid - 1;
+        }
+    }
+    
+    // 确保结果不超过uint32_t范围
+    if (result > 0xFFFFFFFF) {
+        return 0xFFFFFFFF;
+    }
+    
+    return (uint32_t)result;
+}
 void Calculate_PluseNum_Simplified(PLSR_RouteConfig_t *route)
 {
     int32_t accel_pulse_num = 0;    // 加速过程脉冲数
@@ -1456,6 +1493,8 @@ void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
                     g_plsr_route.current_part = PLSR_PART_2;
                     g_plsr_route.run_state = g_plsr_route.part2_state;
                     g_plsr_route.target_freq = g_plsr_route.part2_target_freq;
+                    g_plsr_route.initial_freq = g_plsr_route.current_freq; // 更新加减速初始频率
+                    g_last_freq = 0; // 清零g_last_freq，防止频率计算时的累加效果
                     
                     __HAL_TIM_SetAutoreload(&htim2, g_plsr_route.const_pulse_count);
                     __HAL_TIM_SET_COUNTER(&htim2, 0);
@@ -1467,6 +1506,8 @@ void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
                     g_plsr_route.current_part = PLSR_PART_3;
                     g_plsr_route.run_state = g_plsr_route.part3_state;
                     g_plsr_route.target_freq = g_plsr_route.part3_target_freq;
+                    g_plsr_route.initial_freq = g_plsr_route.current_freq; // 更新加减速初始频率
+                    g_last_freq = 0; // 清零g_last_freq，防止频率计算时的累加效果
                     
                     __HAL_TIM_SetAutoreload(&htim2, g_plsr_route.decel_pulse_count);
                     __HAL_TIM_SET_COUNTER(&htim2, 0);
@@ -1579,6 +1620,7 @@ void PLSR_Route_Set(PLSR_RouteConfig_t* route)
 
     route->start_freq = 0;                       // 起始频率：0Hz
     route->end_freq = 0;                         // 结束频率：0Hz
+    route->initial_freq = 0;                     // 加减速初始频率：0Hz
 
     // 初始化所有段的配置 - 遍历并初始化每个段
     for (uint8_t i = 0; i < route->section_num; i++) 
@@ -1601,6 +1643,7 @@ void PLSR_Route_Init(PLSR_RouteConfig_t* route)
     route->route_state = PLSR_ROUTE_IDLE;        // 路径状态：空闲
     route->current_freq = 0;                     // 当前频率：0Hz
     route->target_freq = 0;                      // 目标频率：0Hz
+    route->initial_freq = 0;                     // 加减速初始频率：0Hz
 
     route->direction = PLSR_DIR_FORWARD;         // 运行方向：默认正向
     
@@ -1623,7 +1666,10 @@ void PLSR_Route_Init(PLSR_RouteConfig_t* route)
     route->prevPulseCount = 0;                   // 累积脉冲计数：清零
     route->pulse_count = 0;                      // 当前脉冲计数：清零
 
-    PLSR_TIM6_SetUpdateFreq(100); //初始化TIM6更新频率为1000us
+    // 初始化全局变量
+    g_last_freq = 0;                             // 清零上一次计算的频率
+
+    PLSR_TIM6_SetUpdateFreq(50); //初始化TIM6更新频率为1000us(1ms)
 }
 
 /**
@@ -1646,6 +1692,7 @@ void PLSR_Route_Start(PLSR_RouteConfig_t* route)
     route->route_state = PLSR_ROUTE_RUNNING;                 // 设置路径状态为运行中
     route->current_section_num = route->start_section;       // 从起始段开始执行
     route->current_freq = route->start_freq;                 // 设置当前频率为起始频率
+    route->initial_freq = route->start_freq;                 // 设置加减速初始频率为起始频率
     route->run_state = PLSR_STATE_IDLE;                      // 设置运行状态为空闲
     
     // 重置全局脉冲计数器
@@ -1681,6 +1728,7 @@ void PLSR_Route_Stop(PLSR_RouteConfig_t* route)
     route->route_state = PLSR_ROUTE_COMPLETED;
     route->run_state = PLSR_STATE_IDLE;
     route->current_freq = 0;
+    route->initial_freq = 0;                     // 重置加减速初始频率
     
     // 重置计数器
     __HAL_TIM_SET_COUNTER(&htim2, 0);
@@ -1809,8 +1857,11 @@ void PLSR_Section_StartNewSection(PLSR_RouteConfig_t* route)
     
     // 如果方向发生变化，将当前频率设为0
     // 这样可以确保在方向切换后，电机从停止状态开始加速，避免方向切换时的冲击
-    if (direction_changed) {
+    if (direction_changed) 
+    {
         route->current_freq = 0;
+        route->initial_freq = 0;  // 同时重置加减速初始频率
+        g_last_freq = 0;  // 清零g_last_freq，防止频率计算时的累加效果
     }
     
     // 将负脉冲数转换为正脉冲数用于计算
@@ -1942,12 +1993,7 @@ void PLSR_Section_StartNewSection(PLSR_RouteConfig_t* route)
     // 为等待时间计数赋值
     PLSR_Wait_StartTimer(route);
 }
-/**
- * @brief  设置三部分运动执行参数
- * @param  route: 路径控制结构体指针
- * @retval None
- * @note   根据当前应该执行的部分设置TIM2参数
- */
+
 /**
  * @brief 设置三段式执行参数（消除递归调用）
  * @param[in,out] route PLSR路径配置结构体指针
@@ -2090,22 +2136,138 @@ void PLSR_Section_SwitchNext(PLSR_RouteConfig_t* route, uint8_t is_pulse_complet
     // 外部事件触发时保持当前频率不变，确保频率连续性
 }
 
+/**
+ * @brief 根据当前脉冲位置计算频率
+ * @param route 路径控制结构体指针
+ * @param is_accel 是否为加速过程(1-加速, 0-减速)
+ * @return 计算得到的频率值
+ * @note 计算当前脉冲的下一个脉冲频率，对于第一个脉冲，使用sqrt(2*a*x)/2作为起始频率
+ */
+uint32_t PLSR_Calculate_FreqByPosition(PLSR_RouteConfig_t* route, uint8_t is_accel)
+{
+    if (route == NULL) return 0;
+    
+    // 使用全局变量g_last_freq替代static变量
+    // 获取当前段配置
+    PLSR_SectionConfig_t* current_section = &route->section[route->current_section_num - 1];
+    
+    // 获取当前脉冲位置
+    uint32_t current_tim2_count = __HAL_TIM_GET_COUNTER(&htim2);
+    
+    // 计算当前部分已经执行的脉冲数
+    uint32_t executed_pulses = current_tim2_count;
+    
+    // 使用速度位移公式 vt^2 = v0^2 + 2ax 计算当前频率，其中v0使用initial_freq作为初始速度
+    uint32_t v0 = route->initial_freq; // 使用initial_freq作为初始速度v0
+    uint64_t a = 0;
+    
+    if (is_accel) 
+    {
+        // 加速过程
+        a = route->accel_rate * 1000ULL; // Hz/ms -> Hz/s^2
+    } 
+    else
+    {
+        a = route->decel_rate * 1000ULL; // 减速过程
+    }
+    
+    // 计算当前脉冲开始时的频率和结束时的频率
+    uint32_t freq_start = 0;  // 当前脉冲开始时的频率
+    uint32_t freq_end = 0;    // 当前脉冲结束时的频率
+    
+    // 计算当前脉冲开始时的频率（executed_pulses位置）
+    if (executed_pulses == 0) 
+    {
+        freq_start = v0;  // 第一个脉冲开始时就是初始频率
+    } 
+    else 
+    {
+        if (is_accel) 
+        {
+            uint64_t v0_squared = (uint64_t)v0 * v0;
+            uint64_t freq_squared_start = v0_squared + 2 * a * executed_pulses;
+            freq_start = integer_sqrt_64(freq_squared_start);
+        } 
+        else 
+        {
+            uint64_t v0_squared = (uint64_t)v0 * v0;
+            if (v0_squared >= 2 * a * executed_pulses) 
+            {
+                uint64_t freq_squared_start = v0_squared - 2 * a * executed_pulses;
+                freq_start = integer_sqrt_64(freq_squared_start);
+            } 
+            else 
+            {
+                freq_start = 0;
+            }
+        }
+    }
+    
+    // 计算当前脉冲结束时的频率（executed_pulses + 1位置）
+    if (is_accel) 
+    {
+        uint64_t v0_squared = (uint64_t)v0 * v0;
+        uint64_t freq_squared_end = v0_squared + 2 * a * (executed_pulses + 1);
+        freq_end = integer_sqrt_64(freq_squared_end);
+    } 
+    else 
+    {
+        uint64_t v0_squared = (uint64_t)v0 * v0;
+        if (v0_squared >= 2 * a * (executed_pulses + 1)) 
+        {
+            uint64_t freq_squared_end = v0_squared - 2 * a * (executed_pulses + 1);
+            freq_end = integer_sqrt_64(freq_squared_end);
+        } 
+        else 
+        {
+            freq_end = 0;
+        }
+    }
+    
+    // 计算当前脉冲的平均频率
+    uint32_t calculated_freq = (freq_start + freq_end) / 2;
+    
+    // 更新全局变量（保存当前脉冲结束时的频率，供下次使用）
+    g_last_freq = freq_end;
+    
+    // 限制频率范围
+    if (calculated_freq < PLSR_PWM_FREQ_MIN) 
+    {
+        calculated_freq = PLSR_PWM_FREQ_MIN;
+    } 
+    else if (calculated_freq > PLSR_PWM_FREQ_MAX) 
+    {
+        calculated_freq = PLSR_PWM_FREQ_MAX;
+    }
+    
+    return calculated_freq;
+}
+
 /**
  * @brief  加减速处理函数
  * @param  route: 路径控制结构体指针
  * @retval None
- * @note   在TIM6中断中调用，处理加减速过程
  */
 void PLSR_Accel_Process(PLSR_RouteConfig_t* route)
 {
     // 参数有效性检查
     if (route == NULL) return;
-    
-    // TIM6更新周期(ms)
-    float tim6_period_ms = (float)s_tim6_update_freq_us / 1000.0f;
-    if (tim6_period_ms == 0) tim6_period_ms = 1;  // 避免除零错误
+
+    // 记录是否是第一次设置非零频率
     uint8_t first_flag = (route->current_freq == 0);
     
+    // 记录上一次的运行状态，用于检测状态变化
+    static PLSR_RunState_t prev_run_state = PLSR_STATE_IDLE;
+    
+    // 检测状态变化，如果状态发生变化，更新initial_freq并清零g_last_freq
+    if (prev_run_state != route->run_state) {
+        // 状态发生变化，将当前频率保存为初始频率
+        route->initial_freq = route->current_freq;
+        // 清零g_last_freq，防止频率计算时的累加效果
+        g_last_freq = 0;
+        prev_run_state = route->run_state;
+    }
+    
     // 加速完成检查
     if (route->run_state == PLSR_STATE_ACCEL && route->current_freq >= route->target_freq) 
     {
@@ -2121,42 +2283,39 @@ void PLSR_Accel_Process(PLSR_RouteConfig_t* route)
         PLSR_PWM_SetFrequency(route->current_freq);
         return;
     }
+    
     // ==================== 加速处理 ====================
     if (route->run_state == PLSR_STATE_ACCEL) 
     {
-        // 计算频率增量 = 加速度 * 时间间隔
-        uint32_t freq_increment = (uint32_t)(route->accel_rate * tim6_period_ms);
+        // 根据当前脉冲位置计算频率（传入1表示加速过程）
+        uint32_t calculated_freq = PLSR_Calculate_FreqByPosition(route, 1);
         
         // 更新当前频率
-        route->current_freq += freq_increment;
+        route->current_freq = calculated_freq;
         
         // 检查是否达到目标频率
         if (route->current_freq >= route->target_freq) {
             route->current_freq = route->target_freq;  // 限制到目标频率
         }
+        
         // 更新PWM频率
         PLSR_PWM_SetFrequency(route->current_freq);
+        
+        // 如果是第一次设置非零频率，启动PWM输出
         if(first_flag == 1 && route->current_freq != 0)
         {
-            first_flag = 0;
             PLSR_PWM_Start();
         }
-        
     } 
     // ==================== 减速处理 ====================
     else if (route->run_state == PLSR_STATE_DECEL) 
     {
-        // 计算频率减量 = 减速度 * 时间间隔
-        uint32_t freq_decrement = (uint32_t)(route->decel_rate * tim6_period_ms);
+        // 根据当前脉冲位置计算频率（传入0表示减速过程）
+        // 使用速度位移公式 vt^2 = v0^2 - 2ax 计算当前脉冲的下一个脉冲频率
+        uint32_t calculated_freq = PLSR_Calculate_FreqByPosition(route, 0);
         
         // 更新当前频率
-        if (route->current_freq > freq_decrement) {
-            route->current_freq -= freq_decrement;
-        } 
-        else 
-        {
-            route->current_freq = 0;  // 防止频率变为负数
-        }
+        route->current_freq = calculated_freq;
         
         // 检查是否达到目标频率
         if (route->current_freq <= route->target_freq) {
@@ -2182,7 +2341,6 @@ void PLSR_Accel_Process(PLSR_RouteConfig_t* route)
             PLSR_PWM_Stop();
         }
     }
-    
 }
 
 /**
@@ -2190,6 +2348,8 @@ void PLSR_Accel_Process(PLSR_RouteConfig_t* route)
  * @param  route: 路径控制结构体指针
  * @retval None
  * @note   根据默认频率和加减速时间重新计算加减速度
+ *         计算得到的加速度单位为Hz/ms，在PLSR_Calculate_FreqByPosition函数中
+ *         会将其转换为Hz/s^2以符合物理公式要求
  */
 void PLSR_Accel_UpdateRates(PLSR_RouteConfig_t* route)
 {