250 lines
7.6 KiB
C++
250 lines
7.6 KiB
C++
// 使用了SR04超声波传感器、TRCT5000循迹传感器、SG90舵机和直流电机进行循迹避障测试
|
||
// 逻辑: 第一次检测到障碍物后开始循迹，第二次检测到障碍物后停止。
|
||
|
||
#include <Servo.h>
|
||
|
||
// --- Pin Definitions ---
|
||
|
||
// Servo
|
||
Servo myServo;
|
||
const int servoPin = 9;
|
||
|
||
// Ultrasonic Sensor - Front (for start/stop control)
|
||
const int frontTrigPin = 2;
|
||
const int frontEchoPin = 3;
|
||
|
||
// Ultrasonic Sensor - Side (for pausing)
|
||
const int sideTrigPin = 8;
|
||
const int sideEchoPin = 6;
|
||
|
||
// LED Pin
|
||
const int ledPin = A2;
|
||
|
||
// Line Following Sensors
|
||
const int trct5000_02 = A1; // 左循迹传感器
|
||
const int trct5000_01 = A0; // 右循迹传感器
|
||
|
||
// Motor Control Pins (assuming L298N-like driver)
|
||
// Right Motor (User identified pin 5 as right motor)
|
||
const int ENA = 4; // 使能A
|
||
const int IN1 = 5; // 输入1 (速度)
|
||
const int IN2 = 10; // 输入2 (方向)
|
||
// Left Motor
|
||
const int ENB = 7; // 使能B
|
||
const int IN3 = 12; // 输入3 (速度)
|
||
const int IN4 = 11; // 输入4 (方向)
|
||
|
||
|
||
// --- Global Variables & Constants ---
|
||
|
||
float frontDistance;
|
||
float sideDistance;
|
||
const int OBSTACLE_THRESHOLD = 20; // 障碍物检测阈值 (cm)
|
||
int obstacleDetections = 0;
|
||
bool obstaclePreviouslySeen = false;
|
||
bool ignoreSideSensor = false; // 新增：用于在恢复后忽略侧方传感器
|
||
int sideObstacleCounter = 0; // 新增：用于过滤侧方传感器的抖动
|
||
int frontStopCounter = 0; // 新增：用于最终停止的防抖
|
||
|
||
// 定义状态机
|
||
enum State {
|
||
IDLE,
|
||
LINE_FOLLOWING,
|
||
PAUSED_BY_SIDE_OBSTACLE, // 新增：被侧方障碍物暂停的状态
|
||
STOPPED
|
||
};
|
||
State currentState = IDLE;
|
||
|
||
// --- Function Prototypes ---
|
||
void motorStop();
|
||
void motorForward(int speed);
|
||
|
||
|
||
void setup() {
|
||
Serial.begin(9600);
|
||
myServo.attach(servoPin, 500, 2500);
|
||
|
||
// 初始化超声波传感器引脚
|
||
pinMode(frontTrigPin, OUTPUT);
|
||
pinMode(frontEchoPin, INPUT);
|
||
pinMode(sideTrigPin, OUTPUT);
|
||
pinMode(sideEchoPin, INPUT);
|
||
|
||
// 初始化LED引脚
|
||
pinMode(ledPin, OUTPUT);
|
||
digitalWrite(ledPin, HIGH); // 初始为高电平，灯灭
|
||
|
||
// 初始化循迹传感器引脚
|
||
pinMode(trct5000_01, INPUT);
|
||
pinMode(trct5000_02, INPUT);
|
||
|
||
// 初始化电机控制引脚
|
||
pinMode(ENA, OUTPUT);
|
||
pinMode(IN1, OUTPUT);
|
||
pinMode(IN2, OUTPUT);
|
||
pinMode(ENB, OUTPUT);
|
||
pinMode(IN3, OUTPUT);
|
||
pinMode(IN4, OUTPUT);
|
||
|
||
// 设置初始状态
|
||
motorStop();
|
||
myServo.write(90); // 舵机居中
|
||
Serial.println("System Initialized. State: IDLE");
|
||
Serial.println("Waiting 3 seconds for sensors to stabilize...");
|
||
delay(3000); // 增加延时以确保传感器稳定
|
||
}
|
||
|
||
void loop() {
|
||
// 1. 读取前方传感器 (用于主流程控制)
|
||
digitalWrite(frontTrigPin, LOW);
|
||
delayMicroseconds(2);
|
||
digitalWrite(frontTrigPin, HIGH);
|
||
delayMicroseconds(10);
|
||
digitalWrite(frontTrigPin, LOW);
|
||
frontDistance = pulseIn(frontEchoPin, HIGH) / 58.00;
|
||
|
||
// 2. 状态机主要逻辑
|
||
switch (currentState) {
|
||
case IDLE:
|
||
motorStop();
|
||
Serial.println("State: IDLE");
|
||
// 检测前方障碍物以启动
|
||
if (frontDistance < OBSTACLE_THRESHOLD && frontDistance > 0 && !obstaclePreviouslySeen) {
|
||
obstacleDetections++;
|
||
Serial.print("Obstacle detected! Count: ");
|
||
Serial.println(obstacleDetections);
|
||
currentState = LINE_FOLLOWING;
|
||
Serial.println("State change -> LINE_FOLLOWING");
|
||
}
|
||
break;
|
||
|
||
case LINE_FOLLOWING:
|
||
// 检查前方障碍物以进入最终停止状态 (增加防抖逻辑)
|
||
if (frontDistance < OBSTACLE_THRESHOLD && frontDistance > 0) {
|
||
frontStopCounter++;
|
||
} else {
|
||
frontStopCounter = 0;
|
||
}
|
||
|
||
if (frontStopCounter >= 5) { // 要求连续5次检测到才触发最终停止
|
||
currentState = STOPPED;
|
||
Serial.println("State change -> STOPPED");
|
||
break; // 退出 switch 语句
|
||
}
|
||
|
||
// 检查侧方障碍物以进入暂停状态 (增加防抖逻辑)
|
||
if (!ignoreSideSensor) {
|
||
digitalWrite(sideTrigPin, LOW);
|
||
delayMicroseconds(2);
|
||
digitalWrite(sideTrigPin, HIGH);
|
||
delayMicroseconds(10);
|
||
digitalWrite(sideTrigPin, LOW);
|
||
sideDistance = pulseIn(sideEchoPin, HIGH) / 58.00;
|
||
|
||
if (sideDistance < OBSTACLE_THRESHOLD && sideDistance > 0) {
|
||
sideObstacleCounter++; // 如果检测到，计数器加1
|
||
} else {
|
||
sideObstacleCounter = 0; // 如果没检测到，计数器清零
|
||
}
|
||
|
||
if (sideObstacleCounter >= 3) { // 要求连续3次检测到才触发暂停
|
||
currentState = PAUSED_BY_SIDE_OBSTACLE;
|
||
Serial.println("State change -> PAUSED_BY_SIDE_OBSTACLE");
|
||
sideObstacleCounter = 0; // 重置计数器
|
||
break;
|
||
}
|
||
}
|
||
|
||
// 执行循迹
|
||
motorForward(90); // 采用用户设定的速度
|
||
|
||
// 读取循迹传感器
|
||
bool leftOnLine = digitalRead(trct5000_02);
|
||
bool rightOnLine = digitalRead(trct5000_01);
|
||
|
||
// 使用舵机进行方向控制 (采用用户设定的角度)
|
||
if (leftOnLine && !rightOnLine) {
|
||
myServo.write(75);
|
||
} else if (!leftOnLine && rightOnLine) {
|
||
myServo.write(105);
|
||
}
|
||
break;
|
||
|
||
case PAUSED_BY_SIDE_OBSTACLE:
|
||
motorStop();
|
||
digitalWrite(ledPin, LOW); // 亮灯
|
||
Serial.println("State: PAUSED");
|
||
|
||
// 在暂停状态下，只检测前方障碍物以恢复运动
|
||
if (frontDistance < OBSTACLE_THRESHOLD && frontDistance > 0) {
|
||
digitalWrite(ledPin, HIGH); // 灭灯
|
||
ignoreSideSensor = true; // 设置标志位，永久忽略侧方传感器
|
||
currentState = LINE_FOLLOWING;
|
||
Serial.println("Resuming, ignoring side sensor from now on.");
|
||
// 为了防止这次检测被错误地计为第二次停止指令，短暂延时并重置检测标志
|
||
delay(200);
|
||
obstaclePreviouslySeen = true;
|
||
}
|
||
break;
|
||
|
||
case STOPPED:
|
||
motorStop();
|
||
break;
|
||
}
|
||
|
||
// 更新前方障碍物的检测标志位，用于识别"新"的障碍物事件
|
||
obstaclePreviouslySeen = (frontDistance < OBSTACLE_THRESHOLD && frontDistance > 0);
|
||
|
||
delay(10); // 采用用户设定的循环延时
|
||
}
|
||
|
||
// --- Motor Control Functions ---
|
||
|
||
// 电机停止 (根据L298N标准，将输入引脚设为LOW实现刹车)
|
||
void motorStop() {
|
||
// 将方向控制引脚都设为LOW，使电机刹车
|
||
digitalWrite(IN1, LOW);
|
||
digitalWrite(IN2, LOW);
|
||
digitalWrite(IN3, LOW);
|
||
digitalWrite(IN4, LOW);
|
||
|
||
// 为保险起见，也将使能引脚的PWM输出设为0
|
||
analogWrite(ENA, 0);
|
||
analogWrite(ENB, 0);
|
||
}
|
||
|
||
// 电机前进 (实现"kick-start"以在低速下启动)
|
||
void motorForward(int speed) {
|
||
// 定义电机启动阈值和"kick"参数
|
||
const int START_THRESHOLD = 140; // 电机启动的最小PWM值
|
||
const int KICK_SPEED = 180; // 用于克服惯性的瞬时启动速度
|
||
const int KICK_DURATION = 20; // 启动脉冲的持续时间 (ms)
|
||
|
||
// 1. 设置电机方向为前进 (根据用户最新配置)
|
||
// 右电机
|
||
digitalWrite(IN1, HIGH);
|
||
digitalWrite(IN2, LOW);
|
||
// 左电机 (用户已反转)
|
||
digitalWrite(IN3, LOW);
|
||
digitalWrite(IN4, HIGH);
|
||
|
||
// 2. 使用PWM在Enable引脚上设置速度, 对低速进行"kick-start"
|
||
if (speed > 0 && speed < START_THRESHOLD) {
|
||
// 先用一个较高的速度"kick"一下来克服静摩擦力
|
||
analogWrite(ENA, KICK_SPEED);
|
||
analogWrite(ENB, KICK_SPEED);
|
||
delay(KICK_DURATION);
|
||
|
||
// 然后迅速降低到目标低速来维持运动
|
||
analogWrite(ENA, speed);
|
||
analogWrite(ENB, speed);
|
||
} else if (speed >= START_THRESHOLD) {
|
||
// 对于高于阈值的速度，直接设置
|
||
analogWrite(ENA, speed);
|
||
analogWrite(ENB, speed);
|
||
} else { // speed == 0
|
||
// 如果目标速度为0，则调用刹车函数
|
||
motorStop();
|
||
}
|
||
}
|