#include "stm32f10x.h"
#include "dataprocess.h"
#include "sensor.h"
#include "baisctime.h"
#include "delay.h"
#include "pwm.h"
#include "encoder.h"
#include "stdlib.h"
/* -------------------- 核心宏定义(优化后) -------------------- */
// 基础定义
#define BLACK 1
#define WHITE 0
#define SENSOR_NUM 7
#define TURNMAX 450 // 最大转向PWM限幅
#define PWMMAX 1000 // 最大电机PWM
#define ENCODER_SPEED_KP 20 // 增强比例系数(原40→80),提高补偿灵敏度
#define SPEED_DIFF_MAX 60 // 增大最大补偿量(原50→100),允许更大调整幅度
// 任务3角度参数
#define TASK3_TURN_LEFT_ANGLE 15 // 左转目标角度
#define TASK3_TURN_RIGHT_ANGLE 3 // 右转目标角度
#define WHEEL_DISTANCE 16.0f // 实测轮距(cm)
#define PULSE_PER_CM 64 // 每厘米脉冲数(实测值)
#define PULSE_PER_DEGREE ((3.14f * WHEEL_DISTANCE * PULSE_PER_CM) / 360.0f)
#define TASK3_TURN_LEFT_PULSES (TASK3_TURN_LEFT_ANGLE * PULSE_PER_DEGREE)
#define TASK3_TURN_RIGHT_PULSES (TASK3_TURN_RIGHT_ANGLE * PULSE_PER_DEGREE)
// 转向与速度参数
#define TASK3_TURN_KP 2.2 // 转向比例系数
#define TASK3_TURN_SPEED 300 // 转向阶段速度
#define TASK3_STRAIGHT_SPEED 250 // 直行阶段速度
#define TASK3_STRAIGHT1_DISTANCE 250 // 左转后直行距离
#define TASK3_STRAIGHT2_DISTANCE 250 // 右转后直行距离
// 超时停车参数
#define DEFAULT_STOPTIME 15 // 默认超时时间(秒)
// 电机方向定义
#define FRONT 0
#define BACK 1
#define LEFTMOTOR 0
#define RIGHTMOTOR 1
/* -------------------- 全局变量定义 -------------------- */
// 循迹与控制变量
int16_t Position = 0, DataSum = 0, DataNum = 0, Last_Position = 0, Turn_Pwm = 0;
int16_t Turn_Kp1 = 60, Turn_Kd = 8;
int16_t Speed_Pwm = 0, Speed_Max = 450, Speed_Min = 320;
int16_t Speed_AllWhite = 500;
// 编码器与角度变量
int16_t Left_Speed, Right_Speed; // 单位时间内左右电机脉冲数(反映实时速度)
int16_t Encoder_Speed_Diff; // 左右速度差(左-右)
int16_t Encoder_Kp = ENCODER_SPEED_KP; // 编码器比例系数(复用宏定义)
int32_t Left_Instant, Right_Instant; // 左右编码器当前值
int32_t Left_Total_Pulses = 0, Right_Total_Pulses = 0; // 总脉冲数
static int32_t Last_Left_Instant = 0, Last_Right_Instant = 0; // 上一时刻编码器值
int16_t Total_Distance = 0; // 累计行驶距离(脉冲数)
int32_t Current_Angle = 0; // 当前转向角度
int32_t Turn_Accumulated_Pulses = 0; // 转向累计脉冲差
int32_t Turn_Start_Left = 0, Turn_Start_Right = 0; // 转向起始时编码器值
// 电机与系统状态
int16_t LeftMotor_Pwm = 0, RightMotor_Pwm = 0; // 左右电机PWM
uint8_t Car_Run = 0, Car_Stop = 0; // 运行/停止标志
int16_t Stoptime = DEFAULT_STOPTIME; // 超时时间(秒)
int16_t DebugMode = 0; // 调试模式标志
int16_t Task_Num = 3; // 当前任务编号
// 黑白状态计数
uint8_t black_to_white_cnt = 0, white_to_black_cnt = 0; // 黑白切换计数
uint8_t current_state = BLACK, last_state = BLACK; // 当前/上一状态(黑/白)
uint8_t is_all_white = 0; // 全白标志(无黑线)
// 任务3状态机
uint8_t task3_state = 0; // 0:未触发 1:左转中 2:左转后直行 3:右转中 4:右转后直行 5:回正 6:恢复直行
/* -------------------- 蜂鸣器与LED控制 -------------------- */
void Buzzer_LED_Init(void) {
GPIO_InitTypeDef GPIO_InitStructure;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA | RCC_APB2Periph_GPIOB, ENABLE);
// 蜂鸣器引脚(PA12)
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOA, &GPIO_InitStructure);
GPIO_SetBits(GPIOA, GPIO_Pin_12); // 初始关闭
// LED引脚(PB14)
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_14;
GPIO_Init(GPIOB, &GPIO_InitStructure);
GPIO_ResetBits(GPIOB, GPIO_Pin_14); // 初始熄灭
}
// 黑白切换提示(黑→白)
void Buzzer_LED_BlackToWhite(void) {
GPIO_ResetBits(GPIOA, GPIO_Pin_12); // 蜂鸣器响
GPIO_SetBits(GPIOB, GPIO_Pin_14); // LED亮
Delay_ms(100);
GPIO_SetBits(GPIOA, GPIO_Pin_12); // 蜂鸣器关
GPIO_ResetBits(GPIOB, GPIO_Pin_14); // LED灭
}
// 白黑切换提示(白→黑)
void Buzzer_LED_WhiteToBlack(void) {
GPIO_ResetBits(GPIOA, GPIO_Pin_12); // 蜂鸣器响
GPIO_SetBits(GPIOB, GPIO_Pin_14); // LED亮
Delay_ms(100);
GPIO_SetBits(GPIOA, GPIO_Pin_12); // 蜂鸣器关
GPIO_ResetBits(GPIOB, GPIO_Pin_14); // LED灭
}
/* -------------------- 循迹位置计算 -------------------- */
void Car_Position_Cal(void) {
uint8_t i;
DataSum = 0; // 黑线位置总和
DataNum = 0; // 检测到黑线的传感器数量
is_all_white = 1; // 默认全白
for (i = 0; i < SENSOR_NUM; i++) {
if (Sensor_Data[i] == BLACK) { // 检测到黑线
DataSum += (i - 3); // 计算相对中间传感器(i=3)的偏差
DataNum++;
is_all_white = 0; // 非全白
}
}
Last_Position = Position; // 保存上一位置
// 计算当前位置(有黑线时取平均偏差,无黑线时保持上一位置)
Position = (DataNum > 0) ? (DataSum * 2 / DataNum) : Last_Position;
}
/* -------------------- 转向PWM计算(核心优化) -------------------- */
void Turn_Pwm_Cal(void) {
if (Car_Stop) { // 停止状态,转向PWM为0
Turn_Pwm = 0;
return;
}
// 任务1/2全白状态:优先用编码器速度差控制,确保速度一致
if ((Task_Num == 1 || Task_Num == 2) && is_all_white) {
// 计算速度差(左速度 - 右速度):正数表示左快,负数表示右快
Encoder_Speed_Diff = Left_Speed - Right_Speed;
// 比例控制:速度差越大,补偿PWM越大(增强补偿力度)
Turn_Pwm = Encoder_Speed_Diff * Encoder_Kp;
// 限制补偿范围,避免过度调整(扩大最大补偿量)
Turn_Pwm = Limit(Turn_Pwm, -SPEED_DIFF_MAX, SPEED_DIFF_MAX);
return;
}
// 任务1/2非全白状态:保留循迹逻辑(按黑线位置转向)
if ((Task_Num == 1 || Task_Num == 2) && !is_all_white) {
// 比例+微分控制:减少震荡,增强循迹稳定性
Turn_Pwm = Position * Turn_Kp1 + (Position - Last_Position) * Turn_Kd;
Turn_Pwm = Limit(Turn_Pwm, -TURNMAX, TURNMAX);
return;
}
// 任务3逻辑(保持原有状态机)
if (Task_Num == 3) {
// 计算当前转向角度(基于累计脉冲差)
Current_Angle = (Turn_Accumulated_Pulses * 360.0f) / (3.14f * WHEEL_DISTANCE * PULSE_PER_CM);
int32_t pulse_error, left_angle_error, right_angle_error;
int32_t left_diff, right_diff; // 当前与转向起始时的脉冲差
switch (task3_state) {
case 1: // 左转中
{
left_diff = Left_Instant - Turn_Start_Left;
right_diff = Right_Instant - Turn_Start_Right;
Turn_Accumulated_Pulses = left_diff - right_diff; // 左脉冲-右脉冲(正值表示左转)
// 限制最大脉冲差,避免过度转向
if (Turn_Accumulated_Pulses > TASK3_TURN_LEFT_PULSES * 1.5) {
Turn_Accumulated_Pulses = TASK3_TURN_LEFT_PULSES * 1.5;
}
// 计算脉冲误差(目标-当前),比例控制转向PWM
pulse_error = TASK3_TURN_LEFT_PULSES - Turn_Accumulated_Pulses;
Turn_Pwm = pulse_error * TASK3_TURN_KP;
// 达到目标角度(±1度内),切换到直行阶段
if (abs(Current_Angle - TASK3_TURN_LEFT_ANGLE) <= 1) {
task3_state = 2;
Total_Distance = 0; // 重置直行距离计数
}
break;
}
case 2: // 左转后直行
left_diff = Left_Instant - Turn_Start_Left;
right_diff = Right_Instant - Turn_Start_Right;
Turn_Accumulated_Pulses = left_diff - right_diff;
// 弱比例控制:微调角度,保持直行
left_angle_error = TASK3_TURN_LEFT_PULSES - Turn_Accumulated_Pulses;
Turn_Pwm = left_angle_error * (TASK3_TURN_KP / 2);
Turn_Pwm = Limit(Turn_Pwm, -SPEED_DIFF_MAX, SPEED_DIFF_MAX);
// 达到直行距离,结束阶段
if (Total_Distance >= TASK3_STRAIGHT1_DISTANCE) {
task3_state = 0;
}
break;
case 3: // 右转中
{
left_diff = Left_Instant - Turn_Start_Left;
right_diff = Right_Instant - Turn_Start_Right;
Turn_Accumulated_Pulses = left_diff - right_diff; // 负值表示右转
// 限制最大脉冲差,避免过度转向
if (Turn_Accumulated_Pulses < -TASK3_TURN_RIGHT_PULSES * 1.5) {
Turn_Accumulated_Pulses = -TASK3_TURN_RIGHT_PULSES * 1.5;
}
// 计算脉冲误差(目标-当前),比例控制转向PWM
pulse_error = (-TASK3_TURN_RIGHT_PULSES) - Turn_Accumulated_Pulses;
Turn_Pwm = pulse_error * TASK3_TURN_KP;
// 达到目标角度(±1度内),切换到直行阶段
if (abs(Current_Angle + TASK3_TURN_RIGHT_ANGLE) <= 1) {
task3_state = 4;
Total_Distance = 0; // 重置直行距离计数
}
break;
}
case 4: // 右转后直行
left_diff = Left_Instant - Turn_Start_Left;
right_diff = Right_Instant - Turn_Start_Right;
Turn_Accumulated_Pulses = left_diff - right_diff;
// 弱比例控制:微调角度,保持直行
right_angle_error = (-TASK3_TURN_RIGHT_PULSES) - Turn_Accumulated_Pulses;
Turn_Pwm = right_angle_error * (TASK3_TURN_KP / 2);
Turn_Pwm = Limit(Turn_Pwm, -SPEED_DIFF_MAX, SPEED_DIFF_MAX);
// 达到直行距离,进入回正阶段
if (Total_Distance >= TASK3_STRAIGHT2_DISTANCE) {
task3_state = 5;
Turn_Start_Left = Left_Instant;
Turn_Start_Right = Right_Instant;
Turn_Accumulated_Pulses = 0;
}
break;
case 5: // 回正阶段
{
left_diff = Left_Instant - Turn_Start_Left;
right_diff = Right_Instant - Turn_Start_Right;
Turn_Accumulated_Pulses = left_diff - right_diff;
// 目标:回到0角度(脉冲差为0)
pulse_error = 0 - Turn_Accumulated_Pulses;
Turn_Pwm = pulse_error * TASK3_TURN_KP;
// 回正完成(±1度内),进入直行阶段
if (abs(Current_Angle) <= 1) {
task3_state = 6;
Turn_Accumulated_Pulses = 0;
}
break;
}
case 6: // 回正后直行(用编码器保持直线)
Encoder_Speed_Diff = Left_Speed - Right_Speed;
Turn_Pwm = Encoder_Speed_Diff * Encoder_Kp;
Turn_Pwm = Limit(Turn_Pwm, -SPEED_DIFF_MAX, SPEED_DIFF_MAX);
break;
default: // 未触发任务3阶段:用编码器保持直线
Encoder_Speed_Diff = Left_Speed - Right_Speed;
Turn_Pwm = Encoder_Speed_Diff * Encoder_Kp;
Turn_Pwm = Limit(Turn_Pwm, -SPEED_DIFF_MAX, SPEED_DIFF_MAX);
break;
}
Turn_Pwm = Limit(Turn_Pwm, -TURNMAX, TURNMAX);
return;
}
// 其他任务默认逻辑:用编码器保持直线
Encoder_Speed_Diff = Left_Speed - Right_Speed;
Turn_Pwm = Encoder_Speed_Diff * Encoder_Kp;
Turn_Pwm = Limit(Turn_Pwm, -SPEED_DIFF_MAX, SPEED_DIFF_MAX);
}
/* -------------------- 速度PWM计算(优化全白稳定性) -------------------- */
void Speed_Pwm_Cal(void) {
if (Car_Stop) { // 停止状态,速度PWM为0
Speed_Pwm = 0;
return;
}
// 任务1/2全白状态:固定速度基准,确保稳定
if ((Task_Num == 1 || Task_Num == 2) && is_all_white) {
Speed_Pwm = 350; // 固定基准速度(可根据实际调整)
// 强制限制在合理范围,避免受其他逻辑干扰
Speed_Pwm = Limit(Speed_Pwm, 200, 500); // 扩大有效范围,确保速度稳定
return;
}
// 任务1/2非全白状态:循迹时速度随偏差调整(偏差越大速度越慢,增强稳定性)
if ((Task_Num == 1 || Task_Num == 2) && !is_all_white) {
Speed_Pwm = Speed_Max - (Speed_Max - Speed_Min) * Abs(Position) / 6;
Speed_Pwm = Limit(Speed_Pwm, Speed_Min, PWMMAX);
return;
}
// 任务3速度逻辑(保持原有)
if (!is_all_white) { // 任务3非全白:随循迹偏差调整速度
Speed_Pwm = Speed_Max - (Speed_Max - Speed_Min) * Abs(Position) / 6;
Speed_Pwm = Limit(Speed_Pwm, Speed_Min, PWMMAX);
return;
}
if (Task_Num == 3) { // 任务3全白:按阶段设置速度
switch (task3_state) {
case 1: case 3: case 5: // 转向阶段:较低速度,确保转向精准
Speed_Pwm = TASK3_TURN_SPEED;
break;
case 2: case 4: // 直行阶段:中等速度
Speed_Pwm = TASK3_STRAIGHT_SPEED;
break;
case 6: default: // 回正后/未触发:全白默认速度
Speed_Pwm = Speed_AllWhite;
break;
}
Speed_Pwm = Limit(Speed_Pwm, Speed_Min, PWMMAX);
return;
}
// 其他任务默认速度
Speed_Pwm = Speed_AllWhite;
Speed_Pwm = Limit(Speed_Pwm, Speed_Min, PWMMAX);
}
/* -------------------- 电机PWM输出 -------------------- */
void Motor_Pwm_Cal(void) {
if (Car_Stop || DebugMode) { // 停止或调试模式,电机PWM为0
LeftMotor_Pwm = 0;
RightMotor_Pwm = 0;
} else if (Car_Run && Sys_time > 200) { // 运行状态(系统初始化后)
// 计算左右电机PWM:转向PWM为正→左快右慢(右转),为负→左慢右快(左转)
LeftMotor_Pwm = Speed_Pwm + Turn_Pwm;
RightMotor_Pwm = Speed_Pwm - Turn_Pwm;
// 限制PWM在有效范围(0~PWMMAX)
LeftMotor_Pwm = Limit(LeftMotor_Pwm, 0, PWMMAX);
RightMotor_Pwm = Limit(RightMotor_Pwm, 0, PWMMAX);
} else { // 未运行或初始化阶段,电机PWM为0
LeftMotor_Pwm = 0;
RightMotor_Pwm = 0;
}
// 输出PWM到电机(正转)
PWM_Output(LEFTMOTOR, FRONT, LeftMotor_Pwm);
PWM_Output(RIGHTMOTOR, FRONT, RightMotor_Pwm);
}
/* -------------------- 编码器更新(提高频率确保实时性) -------------------- */
void Encoder_Update(void) {
// 读取当前编码器值
Left_Instant = Encoder_GetLeft();
Right_Instant = Encoder_GetRight();
// 计算单位时间内的脉冲数(反映实时速度:脉冲数越多速度越快)
Left_Speed = Left_Instant - Last_Left_Instant;
Right_Speed = Right_Instant - Last_Right_Instant;
// 更新上一时刻编码器值(用于下一周期计算速度)
Last_Left_Instant = Left_Instant;
Last_Right_Instant = Right_Instant;
// 累计总脉冲数
Left_Total_Pulses += Left_Speed;
Right_Total_Pulses += Right_Speed;
// 任务3全白状态:更新转向累计脉冲差
if (Task_Num == 3 && is_all_white) {
if (task3_state == 1 || task3_state == 2) { // 左转阶段
int32_t left_diff = Left_Instant - Turn_Start_Left;
int32_t right_diff = Right_Instant - Turn_Start_Right;
Turn_Accumulated_Pulses = left_diff - right_diff;
} else if (task3_state == 3 || task3_state == 4) { // 右转阶段
int32_t left_diff = Left_Instant - Turn_Start_Left;
int32_t right_diff = Right_Instant - Turn_Start_Right;
Turn_Accumulated_Pulses = left_diff - right_diff;
}
}
// 任务3直行阶段:累计行驶距离(取左右脉冲平均值)
if (Task_Num == 3 && (task3_state == 2 || task3_state == 4)) {
Total_Distance += (Abs(Left_Speed) + Abs(Right_Speed)) / 2;
}
}
/* -------------------- 状态触发与停车逻辑 -------------------- */
void Track_State_Control(void) {
uint8_t black_sensor = 0; // 检测到黑线的传感器数量
for (uint8_t i = 0; i < SENSOR_NUM; i++) {
if (Sensor_Data[i] == BLACK) black_sensor++;
}
// 更新当前状态(黑/白)
current_state = (black_sensor > 0) ? BLACK : WHITE;
// 黑白状态切换时的处理
if (current_state != last_state) {
if (current_state == WHITE && last_state == BLACK) { // 黑→白切换
black_to_white_cnt++; // 计数+1
Buzzer_LED_BlackToWhite(); // 提示音/灯
// 任务3触发转向
if (Task_Num == 3) {
if (black_to_white_cnt == 1 && task3_state == 0) { // 第一次黑→白:触发左转
task3_state = 1;
Turn_Start_Left = Left_Instant;
Turn_Start_Right = Right_Instant;
Turn_Accumulated_Pulses = 0;
} else if (black_to_white_cnt == 2 && task3_state == 0) { // 第二次黑→白:触发右转
task3_state = 3;
Turn_Start_Left = Left_Instant;
Turn_Start_Right = Right_Instant;
Turn_Accumulated_Pulses = 0;
}
}
} else if (current_state == BLACK && last_state == WHITE) { // 白→黑切换
white_to_black_cnt++; // 计数+1
Buzzer_LED_WhiteToBlack(); // 提示音/灯
}
last_state = current_state; // 更新上一状态
}
// 不同任务的停车条件
if (Task_Num == 3 && black_to_white_cnt >= 3 && white_to_black_cnt >= 2) {
Car_Stop = 1; // 任务3停车
Car_Run = 0;
} else if (Task_Num == 1 && black_to_white_cnt >= 1 && white_to_black_cnt >= 1) {
Car_Stop = 1; // 任务1停车
Car_Run = 0;
} else if (Task_Num == 2 && black_to_white_cnt >= 3 && white_to_black_cnt >= 2) {
Car_Stop = 1; // 任务2停车
Car_Run = 0;
}
}
/* -------------------- 超时停车函数 -------------------- */
void StopCar(void) {
// 运行中且未超时,系统时间超过设定时间(毫秒)则触发停车
if (Car_Run && !Car_Stop && Sys_time > (uint32_t)(Stoptime * 1000 + 200)) {
Car_Stop = 1; // 触发超时停车
GPIO_ResetBits(GPIOA, GPIO_Pin_12); // 蜂鸣器报警
Delay_ms(500);
GPIO_SetBits(GPIOA, GPIO_Pin_12);
}
}
/* -------------------- 辅助函数 -------------------- */
// 取绝对值
int32_t Abs(int32_t data) {
return data > 0 ? data : -data;
}
// 限制数据在[min, max]范围内
int32_t Limit(int32_t data, int32_t min, int32_t max) {
return (data > max) ? max : (data < min) ? min : data;
}
/* -------------------- 参数初始化 -------------------- */
void Parameter_Init(void) {
// 初始化速度与转向参数
Speed_Max = 450;
Speed_Min = 320;
Speed_AllWhite = 500;
Turn_Kp1 = 60;
Turn_Kd = 8;
Encoder_Kp = ENCODER_SPEED_KP; // 初始化编码器比例系数
// 初始化系统状态
Car_Run = 0;
Car_Stop = 0;
Stoptime = DEFAULT_STOPTIME;
DebugMode = 0;
Task_Num = 2; // 默认任务2
// 初始化黑白状态计数
black_to_white_cnt = 0;
white_to_black_cnt = 0;
current_state = last_state = BLACK;
is_all_white = 0;
Position = 0;
Last_Position = 0;
// 初始化编码器相关变量
Left_Instant = 0;
Right_Instant = 0;
Last_Left_Instant = 0;
Last_Right_Instant = 0;
Left_Total_Pulses = 0;
Right_Total_Pulses = 0;
Total_Distance = 0;
Current_Angle = 0;
Turn_Accumulated_Pulses = 0;
Turn_Start_Left = 0;
Turn_Start_Right = 0;
// 初始化电机PWM与任务3状态
LeftMotor_Pwm = 0;
RightMotor_Pwm = 0;
task3_state = 0;
// 初始化硬件
Buzzer_LED_Init();
Encoder_Init();
}
任务(1):
路径:小车从A点自动行驶到B点停车。
声光提示:停车时触发一次声光提示。
时间限制:用时不超过15秒。
分值:20分。
任务(2):
路径:小车从A点自动行驶到B点 → 沿半弧线行驶到C点 → 从C点自动行驶到D点 → 沿半弧线行驶到A点停车。
声光提示:每经过一个点(即A、B、C、D点)时触发一次声光提示。
时间限制:完成一圈用时不超过30秒。
分值:20分。
任务(3):
路径:小车从A点自动行驶到C点 → 沿半弧线行驶到B点 → 从B点自动行驶到D点 → 沿半弧线行驶到A点停车。
声光提示:每经过一个点(即A、C、B、D点)时触发一次声光提示。
半弧是黑线,任务3是为了走八字,完善一下程序,改错,完整输出dataprocess