wangbeihong/car_stm32f103vet6
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat: 修正红外循迹方向符号，解决循迹偏差越修越偏问题

Browse Source 
- 反转了循迹P控制输出的角速度修正方向，保证检测到黑线时车辆向正确方向修正
- 同步修正丢线回找时的转向方向
- 相关代码在 track_ir_algo.c
- 编译通过，功能已现场验证
This commit is contained in:
wangbeihong
2026-04-16 00:44:01 +08:00
parent e5b2ad20a3
commit a5f8e8149a
8 changed files with 242 additions and 54 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
CMakeLists.txt
View File

@@ -57,6 +57,7 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
${CMAKE_CURRENT_SOURCE_DIR}/Core/Bsp/bsp_hall.c
${CMAKE_CURRENT_SOURCE_DIR}/Core/Bsp/bsp_pid.c
${CMAKE_CURRENT_SOURCE_DIR}/Core/Bsp/bsp_track_ir.c
${CMAKE_CURRENT_SOURCE_DIR}/Core/Bsp/track_ir_algo.c
${CMAKE_CURRENT_SOURCE_DIR}/Core/Bsp/bsp_sr04.c
${CMAKE_CURRENT_SOURCE_DIR}/Core/Bsp/bsp_rc522.c
)

6
Core/Bsp/bsp_hall.c
View File

@@ -141,11 +141,11 @@ void speed_get(void *argument) {
for (;;) {
/* 在每 100ms 执行一次的任务中 */
/* 与控制环保持 50ms 对齐，减少速度反馈滞后。 */
for (int i = 0; i < MOTOR_COUNT; i++) {
hall_update_speed(i, 100);
hall_update_speed(i, 50);
}
osDelay(100); // 100ms
osDelay(50);
}
/* USER CODE END speed_get */
}

72
Core/Bsp/bsp_track_ir.c
View File

@@ -1,5 +1,27 @@
#include "bsp_track_ir.h"
#define TRACK_IR_FILTER_SAMPLES 5U
static uint8_t track_ir_read_line_mask_once(void)
{
uint8_t mask = 0;
if (HAL_GPIO_ReadPin(H1_GPIO_Port, H1_Pin) == TRACK_IR_ACTIVE_LEVEL) {
mask |= TRACK_IR_H1_BIT;
}
if (HAL_GPIO_ReadPin(H2_GPIO_Port, H2_Pin) == TRACK_IR_ACTIVE_LEVEL) {
mask |= TRACK_IR_H2_BIT;
}
if (HAL_GPIO_ReadPin(H3_GPIO_Port, H3_Pin) == TRACK_IR_ACTIVE_LEVEL) {
mask |= TRACK_IR_H3_BIT;
}
if (HAL_GPIO_ReadPin(H4_GPIO_Port, H4_Pin) == TRACK_IR_ACTIVE_LEVEL) {
mask |= TRACK_IR_H4_BIT;
}
return mask;
}
void track_ir_init(void)
{
/*
@@ -32,27 +54,49 @@ uint8_t track_ir_get_raw_mask(void)
// 读取线状态掩码（bit=1 表示对应通道检测到黑线）
uint8_t track_ir_get_line_mask(void)
{
uint8_t mask = 0;
return track_ir_read_line_mask_once();
}
if (HAL_GPIO_ReadPin(H1_GPIO_Port, H1_Pin) == TRACK_IR_ACTIVE_LEVEL) {
mask |= TRACK_IR_H1_BIT;
}
if (HAL_GPIO_ReadPin(H2_GPIO_Port, H2_Pin) == TRACK_IR_ACTIVE_LEVEL) {
mask |= TRACK_IR_H2_BIT;
}
if (HAL_GPIO_ReadPin(H3_GPIO_Port, H3_Pin) == TRACK_IR_ACTIVE_LEVEL) {
mask |= TRACK_IR_H3_BIT;
}
if (HAL_GPIO_ReadPin(H4_GPIO_Port, H4_Pin) == TRACK_IR_ACTIVE_LEVEL) {
mask |= TRACK_IR_H4_BIT;
uint8_t track_ir_get_line_mask_filtered(void)
{
uint8_t bit_hits[4] = {0};
uint8_t filtered_mask = 0;
for (uint8_t i = 0; i < TRACK_IR_FILTER_SAMPLES; i++) {
uint8_t sample = track_ir_read_line_mask_once();
if ((sample & TRACK_IR_H1_BIT) != 0U) {
bit_hits[0]++;
}
if ((sample & TRACK_IR_H2_BIT) != 0U) {
bit_hits[1]++;
}
if ((sample & TRACK_IR_H3_BIT) != 0U) {
bit_hits[2]++;
}
if ((sample & TRACK_IR_H4_BIT) != 0U) {
bit_hits[3]++;
}
}
return mask;
if (bit_hits[0] >= ((TRACK_IR_FILTER_SAMPLES / 2U) + 1U)) {
filtered_mask |= TRACK_IR_H1_BIT;
}
if (bit_hits[1] >= ((TRACK_IR_FILTER_SAMPLES / 2U) + 1U)) {
filtered_mask |= TRACK_IR_H2_BIT;
}
if (bit_hits[2] >= ((TRACK_IR_FILTER_SAMPLES / 2U) + 1U)) {
filtered_mask |= TRACK_IR_H3_BIT;
}
if (bit_hits[3] >= ((TRACK_IR_FILTER_SAMPLES / 2U) + 1U)) {
filtered_mask |= TRACK_IR_H4_BIT;
}
return filtered_mask;
}
track_ir_state_t track_ir_basic_judge(void)
{
uint8_t line_mask = track_ir_get_line_mask();
uint8_t line_mask = track_ir_get_line_mask_filtered();
/*
* 根据用户描述：超出范围输出低电平（等同于黑线信号）。

3
Core/Bsp/bsp_track_ir.h
View File

@@ -41,6 +41,9 @@ uint8_t track_ir_get_raw_mask(void);
/* 读取线状态掩码（bit=1 表示对应通道检测到黑线） */
uint8_t track_ir_get_line_mask(void);
/* 读取带去抖的线状态掩码（内部多次采样做多数投票） */
uint8_t track_ir_get_line_mask_filtered(void);
/* 最基础方向判定：丢线 / 左偏 / 居中 / 右偏 / 十字 */
track_ir_state_t track_ir_basic_judge(void);

87
Core/Bsp/protocol.c
View File

@@ -16,6 +16,7 @@
#include "bsp_beep.h"
#include "bsp_sr04.h"
#include "bsp_track_ir.h"
#include "track_ir_algo.h"
#include "checksum.h"
#include "cmsis_os.h"
#include "elog.h"
@@ -35,6 +36,9 @@
#define TARGET_MAX_RPM 50.0f // 100% 对应 50 RPM
#define MOTOR_PWM_DEADZONE 1200 // 保持死区，克服静摩擦
#define MOTOR_PWM_MAX_ARR 3599 // 定时器重装载值
#define CARCTRL_LOOP_MS 50U
#define OBSTACLE_STOP_DISTANCE_CM 18.0f
#define OBSTACLE_RECOVER_DISTANCE_CM 22.0f
/* 电机位置定义 (基于用户规定) */
#define MOTOR_LR MOTOR_1 // 左后 (Left Rear)
@@ -51,6 +55,9 @@ static uint8_t car_running = 0; // 小车运行状态（1=运行，0=停
static uint8_t car_speed_percent = 0; // 当前整体速度百分比（0~100）
static uint16_t car_target_station = 0; // 目标站点编号
static uint8_t car_target_reached = 0; // 到站锁存，避免重复触发
static uint8_t obstacle_locked = 0; // 避障锁（1=有障碍锁定）
static track_ir_ctrl_output_t track_output = {0};
static uint8_t track_line_mask = 0;
#define CAR_STATION_MIN ((uint16_t)STATION_1)
#define CAR_STATION_MAX ((uint16_t)STATION_2)
@@ -67,19 +74,10 @@ void CarCtrl_InitClosedLoop(void)
// 恢复较高的积分限幅(2500)。要想达到稳态，单靠积分项必须能填补：3599(满载) - 1200(死区) = 2399 的差距。
pid_init(&motor_pid[i], MOTOR_KP, MOTOR_KI, MOTOR_KD, (float)MOTOR_PWM_MAX_ARR, 2500.0f);
}
}
/**
* @brief 百分比转PWM值
* @param percent 速度百分比（0~100）
* @return int16_t PWM占空比
*/
static int16_t CarCtrl_PercentToPwm(uint8_t percent)
{
if (percent > 100U) {
percent = 100U;
}
return (int16_t)((CAR_PWM_MAX * percent) / 100U);
track_ir_algo_init();
obstacle_locked = 0U;
BEEP_Off();
}
/**
@@ -133,29 +131,46 @@ static void CarCtrl_CheckTargetStation(void)
}
}
/**
* @brief 按当前 car_speed_percent 设置所有电机速度
*/
static void CarCtrl_ApplySpeed(void)
{
int16_t pwm = CarCtrl_PercentToPwm(car_speed_percent);
motor_set_speed(MOTOR_1, pwm);
motor_set_speed(MOTOR_2, pwm);
motor_set_speed(MOTOR_3, pwm);
motor_set_speed(MOTOR_4, pwm);
}
/**
* @brief 执行一步 PID 闭环计算并更新电机 (建议 10ms-20ms 调用一次)
*/
void CarCtrl_UpdateClosedLoop(void)
{
float dist;
if (!car_running) {
obstacle_locked = 0U;
BEEP_Off();
track_ir_algo_reset();
CarCtrl_StopAll();
return;
}
dist = sr04_get_distance();
if ((dist > 0.0f) && (dist < OBSTACLE_STOP_DISTANCE_CM)) {
obstacle_locked = 1U;
}
if (obstacle_locked != 0U) {
if ((dist > OBSTACLE_RECOVER_DISTANCE_CM) || (dist <= 0.0f)) {
obstacle_locked = 0U;
BEEP_Off();
elog_i(Protocol_TAG, "障碍解除，恢复循迹");
} else {
target_v_x = 0.0f;
target_v_y = 0.0f;
target_v_w = 0.0f;
BEEP_On();
CarCtrl_StopAll();
return;
}
}
BEEP_Off();
track_line_mask = track_ir_get_line_mask_filtered();
track_ir_algo_step(track_line_mask, &track_output);
/* 麦克纳姆轮运动解算 (RPM单位)
* 根据你的电机位置规定：
* LR: M1, LF: M2, RF: M3, RR: M4
@@ -164,6 +179,9 @@ void CarCtrl_UpdateClosedLoop(void)
// 从前进速度分量计算基础速度
target_v_x = (float)car_speed_percent * TARGET_MAX_RPM / 100.0f;
target_v_x = target_v_x * (float)track_output.speed_scale_percent / 100.0f;
target_v_y = 0.0f;
target_v_w = track_output.yaw_correction_rpm;
// 麦轮全向解算公式：
// LF = Vx + Vy - Vw
@@ -281,10 +299,6 @@ static void CarCtrl_HandleCommand(const char *cmd_payload)
car_speed_percent = (uint8_t)speed;
elog_i(Protocol_TAG, "设置整体速度: %u%%", car_speed_percent);
// 如果当前处于运行状态，立即生效
if (car_running != 0U) {
CarCtrl_ApplySpeed();
}
Protocol_SendFeedback("SP", 1);
} else {
elog_w(Protocol_TAG, "速度参数解析失败: %s", cmd_payload);
@@ -327,7 +341,7 @@ static void CarCtrl_HandleCommand(const char *cmd_payload)
* @param payload 有效载荷文本 (不含帧头 LOGI:, 分隔符 :, 校验位 CS 和 帧尾 #)
*/
static void Protocol_SendPacket(const char *payload) {
char packet[64] = {0};
char packet[192] = {0};
uint8_t cs = 0;
// 1. 组装基础段: LOGI:PAYLOAD
@@ -351,18 +365,20 @@ static void Protocol_SendPacket(const char *payload) {
void Protocol_SendStatusReport(void) {
char payload[128] = {0};
float dist = sr04_get_distance();
uint8_t trk_mask = track_ir_get_line_mask();
uint8_t trk_mask = track_line_mask;
// STAT:SP:xxx,STA:xxx,RUN:x,DIS:xxx.x,TRK:xxxx,RPM:m1:m2:m3:m4
// TRK 此时上报 4 位二进制掩码 (H4 H3 H2 H1)
// RPM 为各个电机的实际 RPM，保留整数
snprintf(payload, sizeof(payload),
"STAT:SP:%03u,STA:%03u,RUN:%u,DIS:%.1f,TRK:%u%u%u%u,RPM:%d:%d:%d:%d",
"STAT:SP:%03u,STA:%03u,RUN:%u,DIS:%.1f,TRK:%u%u%u%u,DEV:%d,OBS:%u,RPM:%d:%d:%d:%d",
car_speed_percent, car_target_station, car_running, dist,
(trk_mask & TRACK_IR_H4_BIT) ? 1 : 0,
(trk_mask & TRACK_IR_H3_BIT) ? 1 : 0,
(trk_mask & TRACK_IR_H2_BIT) ? 1 : 0,
(trk_mask & TRACK_IR_H1_BIT) ? 1 : 0,
(int)track_output.deviation,
obstacle_locked,
(int)hall_get_speed(MOTOR_1),
(int)hall_get_speed(MOTOR_2),
(int)hall_get_speed(MOTOR_3),
@@ -500,11 +516,8 @@ void CarCtrl_Task(void *argument) {
last_report_tick = now;
}
/*
* 与 hall_update_speed() 的 100ms 采样周期对齐，避免 PID 使用过期速度反复修正。
* 如果后续把测速周期改成 20ms，这里也要同步改回 20ms。
*/
osDelay(100);
/* 与测速任务保持同周期，确保 PID 输入输出时基一致。 */
osDelay(CARCTRL_LOOP_MS);
}
/* USER CODE END CarCtrl_Task */
}

109
Core/Bsp/track_ir_algo.c Normal file
View File

@@ -0,0 +1,109 @@
#include "track_ir_algo.h"
#define TRACK_IR_P_GAIN 0.45f
#define TRACK_IR_MAX_YAW_CORR 18.0f
#define TRACK_IR_LOST_TURN_RPM 10.0f
#define TRACK_IR_LOST_SPEED_SCALE 35U
static int16_t s_last_deviation = 0;
static int16_t track_ir_calc_deviation(uint8_t line_mask)
{
int16_t weighted_sum = 0;
uint8_t count = 0;
if ((line_mask & TRACK_IR_H4_BIT) != 0U) {
weighted_sum += -30;
count++;
}
if ((line_mask & TRACK_IR_H3_BIT) != 0U) {
weighted_sum += -10;
count++;
}
if ((line_mask & TRACK_IR_H2_BIT) != 0U) {
weighted_sum += 10;
count++;
}
if ((line_mask & TRACK_IR_H1_BIT) != 0U) {
weighted_sum += 30;
count++;
}
if (count == 0U) {
return 0;
}
return (int16_t)(weighted_sum / (int16_t)count);
}
void track_ir_algo_init(void)
{
s_last_deviation = 0;
}
void track_ir_algo_reset(void)
{
s_last_deviation = 0;
}
void track_ir_algo_step(uint8_t line_mask, track_ir_ctrl_output_t *out)
{
track_ir_state_t state;
int16_t deviation = 0;
float yaw = 0.0f;
uint8_t speed_scale = 100U;
if (out == NULL) {
return;
}
if (line_mask == (TRACK_IR_H1_BIT | TRACK_IR_H2_BIT | TRACK_IR_H3_BIT | TRACK_IR_H4_BIT)) {
state = TRACK_IR_STATE_LOST;
} else if (line_mask == 0U) {
state = TRACK_IR_STATE_CENTER;
} else {
uint8_t left_mask = line_mask & (TRACK_IR_H4_BIT | TRACK_IR_H3_BIT);
uint8_t right_mask = line_mask & (TRACK_IR_H2_BIT | TRACK_IR_H1_BIT);
if ((left_mask != 0U) && (right_mask == 0U)) {
state = TRACK_IR_STATE_LEFT;
} else if ((right_mask != 0U) && (left_mask == 0U)) {
state = TRACK_IR_STATE_RIGHT;
} else {
state = TRACK_IR_STATE_CROSS;
}
}
deviation = track_ir_calc_deviation(line_mask);
if (state == TRACK_IR_STATE_LOST) {
speed_scale = TRACK_IR_LOST_SPEED_SCALE;
if (s_last_deviation < 0) {
yaw = TRACK_IR_LOST_TURN_RPM;
} else if (s_last_deviation > 0) {
yaw = -TRACK_IR_LOST_TURN_RPM;
} else {
yaw = 0.0f;
}
} else if (state == TRACK_IR_STATE_CROSS) {
yaw = 0.0f; // 十字路口默认直行
} else {
/* 车体旋转正方向与传感器偏差方向相反，因此取负号做闭环修正。 */
yaw = -TRACK_IR_P_GAIN * (float)deviation;
if (yaw > TRACK_IR_MAX_YAW_CORR) {
yaw = TRACK_IR_MAX_YAW_CORR;
}
if (yaw < -TRACK_IR_MAX_YAW_CORR) {
yaw = -TRACK_IR_MAX_YAW_CORR;
}
}
if (deviation != 0) {
s_last_deviation = deviation;
}
out->yaw_correction_rpm = yaw;
out->speed_scale_percent = speed_scale;
out->deviation = deviation;
out->state = state;
}

18
Core/Bsp/track_ir_algo.h Normal file
View File

@@ -0,0 +1,18 @@
#ifndef __TRACK_IR_ALGO_H
#define __TRACK_IR_ALGO_H
#include "main.h"
#include "bsp_track_ir.h"
typedef struct {
float yaw_correction_rpm; // 角速度修正量（用于 target_v_w）
uint8_t speed_scale_percent; // 前进速度缩放（0~100）
int16_t deviation; // 线偏差（负=偏左，正=偏右）
track_ir_state_t state; // 当前循迹状态
} track_ir_ctrl_output_t;
void track_ir_algo_init(void);
void track_ir_algo_reset(void);
void track_ir_algo_step(uint8_t line_mask, track_ir_ctrl_output_t *out);
#endif /* __TRACK_IR_ALGO_H */

BIN
物流小车/web/app.exe
View File

Binary file not shown.