初次测试单片机循迹避障
This commit is contained in:
@@ -1,99 +1,249 @@
|
||||
//使用了SR04超声波传感器，可收发超声波，测距0~450cm
|
||||
//通过串口显示距离
|
||||
// 使用了SR04超声波传感器、TRCT5000循迹传感器、SG90舵机和直流电机进行循迹避障测试
|
||||
// 逻辑: 第一次检测到障碍物后开始循迹，第二次检测到障碍物后停止。
|
||||
|
||||
#include <Servo.h>
|
||||
|
||||
// --- Pin Definitions ---
|
||||
|
||||
// Servo
|
||||
Servo myServo;
|
||||
const int servoPin = 9;
|
||||
//设定SR04连接的Arduino引脚
|
||||
const int TrigPin = 8; //C语言的基本数据类型中的一种，const int定义的是一个常量，它的值不可以更改。
|
||||
const int EchoPin = 6;
|
||||
const int TrigPin2 = 2;
|
||||
const int EchoPin2 = 3;
|
||||
const int trct5000_01=A0;
|
||||
float distance; //C语言的基本数据类型中的一种，表示单精度浮点数。
|
||||
float distance2;
|
||||
int flag;
|
||||
|
||||
void setup()
|
||||
{
|
||||
Serial.begin(9600); //初始化串口通信
|
||||
myServo.attach(servoPin,500, 2500);
|
||||
pinMode(TrigPin, OUTPUT); //设置4（TrigPin）脚为输出状态
|
||||
// 要检测引脚上输入的脉冲宽度，需要先设置为输入状态
|
||||
pinMode(EchoPin, INPUT); //设置6（EchoPin）脚为输入状态
|
||||
// Ultrasonic Sensor - Front (for start/stop control)
|
||||
const int frontTrigPin = 2;
|
||||
const int frontEchoPin = 3;
|
||||
|
||||
pinMode(TrigPin2, OUTPUT); //设置4（TrigPin）脚为输出状态
|
||||
// 要检测引脚上输入的脉冲宽度，需要先设置为输入状态
|
||||
pinMode(EchoPin2, INPUT); //设置6（EchoPin）脚为输入状态
|
||||
Serial.println("Ultrasonic sensor:"); //串口监视器显示Ultrasonic sensor:
|
||||
pinMode(A0,INPUT);
|
||||
// Ultrasonic Sensor - Side (for pausing)
|
||||
const int sideTrigPin = 8;
|
||||
const int sideEchoPin = 6;
|
||||
|
||||
pinMode(4, OUTPUT); //EA
|
||||
pinMode(7, OUTPUT); //EB
|
||||
pinMode(10, OUTPUT); //A1
|
||||
pinMode(5, OUTPUT); //A1
|
||||
pinMode(11, OUTPUT); //B1
|
||||
pinMode(12, OUTPUT); //B2
|
||||
digitalWrite(4,HIGH);
|
||||
digitalWrite(7,HIGH);
|
||||
myServo.write(90);
|
||||
// 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()
|
||||
{
|
||||
|
||||
analogWrite(5, 200);
|
||||
analogWrite(10, 50);
|
||||
Serial.println(digitalRead(trct5000_01));
|
||||
analogWrite(11, 50);
|
||||
analogWrite(12, 200);
|
||||
digitalWrite(TrigPin, LOW); //设置数字引脚4为低电平
|
||||
delayMicroseconds(2); //产生一个2us延迟，“delayMicroseconds();”为微秒级延时。
|
||||
digitalWrite(TrigPin, HIGH); //设置数字引脚4为高电平
|
||||
delayMicroseconds(10); //产生一个10us的高脉冲去触发TrigPin，至少10us的高电平信号才能触发模块的测距功能。
|
||||
digitalWrite(TrigPin, LOW); //设置数字引脚4为低电平
|
||||
distance = pulseIn(EchoPin, HIGH) / 58.00; //检测脉冲宽度，并计算出距离
|
||||
void loop() {
|
||||
// 1. 读取前方传感器 (用于主流程控制)
|
||||
digitalWrite(frontTrigPin, LOW);
|
||||
delayMicroseconds(2);
|
||||
digitalWrite(frontTrigPin, HIGH);
|
||||
delayMicroseconds(10);
|
||||
digitalWrite(frontTrigPin, LOW);
|
||||
frontDistance = pulseIn(frontEchoPin, HIGH) / 58.00;
|
||||
|
||||
digitalWrite(TrigPin2, LOW); //设置数字引脚4为低电平
|
||||
delayMicroseconds(2); //产生一个2us延迟，“delayMicroseconds();”为微秒级延时。
|
||||
digitalWrite(TrigPin2, HIGH); //设置数字引脚4为高电平
|
||||
delayMicroseconds(10); //产生一个10us的高脉冲去触发TrigPin，至少10us的高电平信号才能触发模块的测距功能。
|
||||
digitalWrite(TrigPin2, LOW); //设置数字引脚4为低电平
|
||||
distance2 = pulseIn(EchoPin2, 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(distance < 10)
|
||||
{
|
||||
flag = 1;
|
||||
digitalWrite(4,LOW);
|
||||
digitalWrite(7,LOW);
|
||||
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;
|
||||
}
|
||||
else
|
||||
{
|
||||
flag = 0;
|
||||
}
|
||||
if(flag == 0)
|
||||
{
|
||||
if(distance2 < 20)
|
||||
{
|
||||
myServo.write(110);
|
||||
digitalWrite(4,LOW);
|
||||
digitalWrite(7,LOW);
|
||||
}
|
||||
else
|
||||
{
|
||||
digitalWrite(4,HIGH);
|
||||
digitalWrite(7,HIGH);
|
||||
}
|
||||
}
|
||||
//注意！！！此语句中58.00前的“/”符号为算术运算符，表示除以。
|
||||
Serial.print(distance); //串口监视器显示distance（所测距离）
|
||||
Serial.println(); //串口监视器打印一行回车
|
||||
Serial.print(distance2); //串口监视器显示distance（所测距离）
|
||||
Serial.print("cm"); //串口监视器显示“cm”
|
||||
Serial.println(); //串口监视器打印一行回车
|
||||
delay(100); //延迟1秒（1000毫秒）
|
||||
|
||||
|
||||
|
||||
|
||||
// 更新前方障碍物的检测标志位，用于识别"新"的障碍物事件
|
||||
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();
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user