实验室紫外灯智能消毒机器人
项目开发背景
实验室环境通常涉及各种生物或化学实验,存在较高的微生物污染风险,尤其是细菌、病毒等病原体可能通过空气或表面传播,威胁实验人员的健康与实验结果的准确性。传统消毒方式如人工喷洒消毒液或固定紫外灯照射,往往效率低下且覆盖不均,难以确保实验室每个角落都得到彻底清洁。此外,紫外灯在使用时若未及时关闭,可能对误入人员造成伤害,因此亟需一种智能化、自动化的解决方案来提升消毒效率和安全性。
紫外灯智能消毒机器人的开发正是基于这一需求,旨在通过自动化技术实现高效、安全的实验室环境维护。该机器人能够自主移动并覆盖预设路径,确保消毒无死角,同时结合红外人体感应模块,在检测到人员活动时自动暂停紫外灯工作,避免潜在的健康风险。这种设计不仅减少了人工干预,还显著降低了因疏忽导致的安全事故,符合现代实验室对智能化和精细化管理的要求。
在功能实现上,机器人通过定时设置和自动返回充电坞的设计,进一步提升了操作的便捷性和持续性。防跌落传感器的加入则确保了机器人在复杂实验室环境中的稳定运行,防止意外跌落造成的设备损坏或安全隐患。这些特性共同构成了一个可靠的消毒系统,能够适应实验室的高标准卫生需求。
硬件模块的集成,如STM32主控芯片和电机驱动系统,为机器人的精准控制和高效执行提供了基础支撑。通过将移动底盘、紫外灯执行器与安全传感器有机结合,该项目不仅推动了实验室消毒技术的升级,还为未来智能机器人在其他领域的应用积累了宝贵经验。总体而言,该开发项目旨在通过创新技术解决现实问题,促进实验室管理的智能化和人性化发展。
设计实现的功能
(1) 使用STM32F103C8T6单片机控制L298N电机驱动板,驱动带编码器的直流减速电机,实现机器人底盘按照预设路径在实验室内移动。
(2) 通过HC-SR501人体红外传感器检测人体存在,STM32在确认无人状态下控制继电器模块开启紫外灯管进行消毒。
(3) STM32具备定时功能,可设置消毒时长,时间结束后控制机器人自动返回充电坞并关闭紫外灯。
(4) 使用红外对管防跌落传感器,STM32实时监测,在检测到跌落风险时采取安全措施,如停止移动。
项目硬件模块组成
(1)主控芯片:STM32F103C8T6单片机。
(2)移动底盘:带编码器的直流减速电机与车轮、L298N电机驱动板。
(3)核心执行器:紫外灯管与继电器模块。
(4)安全传感:HC-SR501人体红外传感器、红外对管防跌落传感器。
设计意义
该实验室紫外灯智能消毒机器人的设计意义在于实现自动化消毒流程,通过预设路径移动功能,机器人能够自主覆盖实验室空间,无需人工干预即可完成消毒任务,显著提升消毒效率并降低人力成本。同时,机器人集成了红外人体感应模块,确保仅在无人状态下开启紫外灯,有效避免紫外线辐射对人员的潜在危害,保障了实验室操作的安全性。
防跌落功能通过红外对管传感器实时监测环境,防止机器人在运行中发生意外跌落,进一步增强了设备的可靠性和使用寿命。定时功能与自动返回充电坞的设计,使得机器人能够精确控制消毒时长并在结束后自主归位,不仅优化了能源管理,还确保了消毒过程的规范化和连续性。
整体而言,该设计结合了移动底盘、智能传感和核心执行器,为实验室环境提供了一种高效、安全的消毒解决方案,有助于维持洁净的实验条件,减少交叉污染风险,并支持科研工作的顺利开展。
设计思路
该机器人的设计以STM32F103C8T6单片机作为核心控制器,负责协调各硬件模块的运行。通过预设程序控制底盘移动、安全检测和消毒执行,确保整个系统高效可靠地工作。
移动底盘采用带编码器的直流减速电机和L298N电机驱动板,实现精确的速度和位置控制。机器人能够按照预设路径在实验室内自主移动,编码器反馈数据用于校正路径偏差,保证移动的准确性。
安全方面,HC-SR501人体红外传感器实时监测环境,仅在检测到无人状态时才允许开启紫外灯,避免对人体造成伤害。同时,红外对管防跌落传感器安装在机器人底部,当检测到边缘或障碍时立即停止移动,防止跌落事故。
消毒功能通过继电器模块控制紫外灯管的开关,结合定时设置实现自动化操作。用户可预设消毒时长,结束后系统自动关闭紫外灯,并引导机器人返回充电坞进行充电,确保能源管理和任务连续性。
整体设计注重实用性和安全性,各模块通过主控芯片集成,实现移动、检测和消毒的协同工作,无需人工干预即可完成实验室的智能消毒任务。
框架图
+-------------------+ +-----------------------+
| | | |
| 人体红外传感器 |------>| |
| (HC-SR501) | | |
+-------------------+ | |
| 主控芯片 |
+-------------------+ | (STM32F103C8T6) |
| |------>| |
| 防跌落传感器 | | |
| (红外对管) | | |
+-------------------+ +-----------------------+
| |
| |
+-------------------+ | |
| |<------| |
| 电机驱动板 | | |
| (L298N) | | |
+-------------------+ | |
| | |
v | |
+-------------------+ | |
| 直流减速电机 | | |
| (带编码器) |------>| |
+-------------------+ | |
| |
+-------------------+ | |
| |<------| |
| 继电器模块 | | |
+-------------------+ | |
| +-----------------------+
v
+-------------------+
| 紫外灯管 |
+-------------------+
系统总体设计
系统总体设计以STM32F103C8T6单片机作为核心控制单元,负责协调和管理机器人的各项功能。移动底盘采用带编码器的直流减速电机与L298N电机驱动板组合,实现机器人在实验室内按照预设路径自主移动,编码器提供位置反馈以确保移动精度。
消毒功能通过HC-SR501人体红外传感器检测环境状态,仅在确认无人时才会通过继电器模块开启紫外灯管进行消毒。系统集成定时功能,允许用户设置消毒时长,消毒结束后机器人自动返回充电坞并关闭紫外灯,实现全自动化操作。
安全方面,系统配备红外对管防跌落传感器,实时监测机器人周围环境,防止在移动过程中从高处跌落,确保运行过程的稳定性和安全性。整体设计注重实际应用,各硬件模块协同工作,满足实验室消毒的可靠需求。
系统功能总结
| 编号 | 功能描述 | 实现硬件 |
|---|---|---|
| 1 | 机器人底盘可按照预设路径在实验室内移动。 | 移动底盘:带编码器的直流减速电机与车轮、L298N电机驱动板 |
| 2 | 通过红外人体感应模块确保在无人状态下才开启紫外灯进行消毒。 | HC-SR501人体红外传感器、紫外灯管与继电器模块 |
| 3 | 具备定时功能,可设置消毒时长,结束后自动返回充电坞并关闭紫外灯。 | 主控芯片STM32F103C8T6(控制定时和逻辑)、移动底盘(返回动作) |
| 4 | 具备防跌落功能,确保运行安全。 | 红外对管防跌落传感器 |
设计的各个功能模块描述
主控芯片模块基于STM32F103C8T6单片机,作为整个系统的控制核心,负责协调各模块的运行。它处理来自传感器的输入信号,控制移动底盘按照预设路径移动,并管理定时功能,实现在设置消毒时长结束后自动返回充电坞并关闭紫外灯。同时,该芯片通过逻辑判断确保仅在无人状态下才允许开启紫外灯,以保障安全。
移动底盘模块由带编码器的直流减速电机、车轮和L298N电机驱动板组成,实现机器人在实验室内的自主移动。编码器提供位置反馈,帮助精确控制移动轨迹,而L298N驱动板调节电机的转速和方向,确保底盘能够稳定地沿预设路径运行,并在需要时返回充电坞。
核心执行器模块包括紫外灯管和继电器模块,紫外灯管用于进行消毒操作,继电器模块在主控芯片的控制下开关紫外灯。该模块仅在红外人体感应模块确认无人状态时被激活,从而避免对人员造成伤害,并在定时结束后自动关闭紫外灯。
安全传感模块由HC-SR501人体红外传感器和红外对管防跌落传感器构成。人体红外传感器检测实验室内是否有人存在,确保紫外灯只在无人环境下开启;防跌落传感器通过红外对管监测地面边缘或障碍物,防止机器人在移动过程中发生跌落,保障运行安全。
上位机代码设计
// main.cpp
#include <QApplication>
#include <QMainWindow>
#include <QVBoxLayout>
#include <QHBoxLayout>
#include <QPushButton>
#include <QLabel>
#include <QLineEdit>
#include <QTextEdit>
#include <QGroupBox>
#include <QSerialPort>
#include <QSerialPortInfo>
#include <QTimer>
#include <QMessageBox>
#include <QProgressBar>
#include <QSpinBox>
#include <QCheckBox>
#include <QDateTime>
class RobotController : public QMainWindow
{
Q_OBJECT
public:
RobotController(QWidget *parent = nullptr) : QMainWindow(parent)
{
setupUI();
setupSerialPort();
setupConnections();
}
~RobotController()
{
if(serialPort->isOpen())
serialPort->close();
}
private slots:
void connectRobot()
{
serialPort->setPortName(portComboBox->currentText());
serialPort->setBaudRate(QSerialPort::Baud115200);
if(serialPort->open(QIODevice::ReadWrite))
{
statusLabel->setText("已连接到机器人");
logTextEdit->append("[" + QDateTime::currentDateTime().toString("hh:mm:ss") + "] 机器人连接成功");
}
else
{
QMessageBox::warning(this, "连接失败", "无法连接到串口设备");
}
}
void disconnectRobot()
{
if(serialPort->isOpen())
{
serialPort->close();
statusLabel->setText("未连接");
logTextEdit->append("[" + QDateTime::currentDateTime().toString("hh:mm:ss") + "] 断开连接");
}
}
void startDisinfection()
{
if(!serialPort->isOpen())
{
QMessageBox::warning(this, "错误", "请先连接机器人");
return;
}
int duration = durationSpinBox->value();
QString command = QString("START:%1").arg(duration);
serialPort->write(command.toUtf8());
logTextEdit->append("[" + QDateTime::currentDateTime().toString("hh:mm:ss") + "] 开始消毒,时长: " + QString::number(duration) + "分钟");
// 启动进度条更新
progressBar->setMaximum(duration * 60); // 转换为秒
progressBar->setValue(0);
// 启动定时器更新进度
progressTimer->start(1000);
}
void stopDisinfection()
{
if(serialPort->isOpen())
{
serialPort->write("STOP");
logTextEdit->append("[" + QDateTime::currentDateTime().toString("hh:mm:ss") + "] 停止消毒");
progressTimer->stop();
progressBar->setValue(0);
}
}
void returnToDock()
{
if(serialPort->isOpen())
{
serialPort->write("RETURN");
logTextEdit->append("[" + QDateTime::currentDateTime().toString("hh:mm:ss") + "] 返回充电坞");
}
}
void updateProgress()
{
int current = progressBar->value() + 1;
progressBar->setValue(current);
if(current >= progressBar->maximum())
{
progressTimer->stop();
logTextEdit->append("[" + QDateTime::currentDateTime().toString("hh:mm:ss") + "] 消毒完成");
}
}
void readSerialData()
{
QByteArray data = serialPort->readAll();
QString message = QString::fromUtf8(data);
if(!message.isEmpty())
{
logTextEdit->append("机器人: " + message.trimmed());
if(message.contains("HUMAN_DETECTED"))
{
QMessageBox::warning(this, "安全警告", "检测到人员活动,紫外灯已关闭");
}
else if(message.contains("DISINFECTION_COMPLETE"))
{
progressTimer->stop();
progressBar->setValue(progressBar->maximum());
logTextEdit->append("[" + QDateTime::currentDateTime().toString("hh:mm:ss") + "] 消毒任务完成");
}
}
}
void refreshPorts()
{
portComboBox->clear();
QList<QSerialPortInfo> ports = QSerialPortInfo::availablePorts();
for(const QSerialPortInfo &port : ports)
{
portComboBox->addItem(port.portName());
}
}
private:
void setupUI()
{
setWindowTitle("实验室紫外灯智能消毒机器人控制系统");
setFixedSize(600, 700);
QWidget *centralWidget = new QWidget(this);
setCentralWidget(centralWidget);
QVBoxLayout *mainLayout = new QVBoxLayout(centralWidget);
// 连接设置组
QGroupBox *connectionGroup = new QGroupBox("连接设置");
QHBoxLayout *connectionLayout = new QHBoxLayout(connectionGroup);
portComboBox = new QComboBox();
refreshButton = new QPushButton("刷新端口");
connectButton = new QPushButton("连接");
disconnectButton = new QPushButton("断开");
connectionLayout->addWidget(new QLabel("串口:"));
connectionLayout->addWidget(portComboBox);
connectionLayout->addWidget(refreshButton);
connectionLayout->addWidget(connectButton);
connectionLayout->addWidget(disconnectButton);
// 控制组
QGroupBox *controlGroup = new QGroupBox("机器人控制");
QVBoxLayout *controlLayout = new QVBoxLayout(controlGroup);
QHBoxLayout *durationLayout = new QHBoxLayout();
durationLayout->addWidget(new QLabel("消毒时长(分钟):"));
durationSpinBox = new QSpinBox();
durationSpinBox->setRange(1, 120);
durationSpinBox->setValue(30);
durationLayout->addWidget(durationSpinBox);
durationLayout->addStretch();
startButton = new QPushButton("开始消毒");
stopButton = new QPushButton("停止消毒");
returnButton = new QPushButton("返回充电坞");
QHBoxLayout *buttonLayout = new QHBoxLayout();
buttonLayout->addWidget(startButton);
buttonLayout->addWidget(stopButton);
buttonLayout->addWidget(returnButton);
progressBar = new QProgressBar();
controlLayout->addLayout(durationLayout);
controlLayout->addLayout(buttonLayout);
controlLayout->addWidget(progressBar);
// 状态显示组
QGroupBox *statusGroup = new QGroupBox("系统状态");
QVBoxLayout *statusLayout = new QVBoxLayout(statusGroup);
statusLabel = new QLabel("未连接");
logTextEdit = new QTextEdit();
logTextEdit->setReadOnly(true);
statusLayout->addWidget(statusLabel);
statusLayout->addWidget(logTextEdit);
// 添加到主布局
mainLayout->addWidget(connectionGroup);
mainLayout->addWidget(controlGroup);
mainLayout->addWidget(statusGroup);
refreshPorts();
}
void setupSerialPort()
{
serialPort = new QSerialPort(this);
progressTimer = new QTimer(this);
}
void setupConnections()
{
connect(connectButton, &QPushButton::clicked, this, &RobotController::connectRobot);
connect(disconnectButton, &QPushButton::clicked, this, &RobotController::disconnectRobot);
connect(refreshButton, &QPushButton::clicked, this, &RobotController::refreshPorts);
connect(startButton, &QPushButton::clicked, this, &RobotController::startDisinfection);
connect(stopButton, &QPushButton::clicked, this, &RobotController::stopDisinfection);
connect(returnButton, &QPushButton::clicked, this, &RobotController::returnToDock);
connect(serialPort, &QSerialPort::readyRead, this, &RobotController::readSerialData);
connect(progressTimer, &QTimer::timeout, this, &RobotController::updateProgress);
}
// UI组件
QComboBox *portComboBox;
QPushButton *refreshButton;
QPushButton *connectButton;
QPushButton *disconnectButton;
QPushButton *startButton;
QPushButton *stopButton;
QPushButton *returnButton;
QSpinBox *durationSpinBox;
QProgressBar *progressBar;
QLabel *statusLabel;
QTextEdit *logTextEdit;
// 功能组件
QSerialPort *serialPort;
QTimer *progressTimer;
};
int main(int argc, char *argv[])
{
QApplication app(argc, argv);
RobotController controller;
controller.show();
return app.exec();
}
#include "main.moc"
# RobotController.pro
QT += core gui serialport
greaterThan(QT_MAJOR_VERSION, 4): QT += widgets
TARGET = RobotController
TEMPLATE = app
SOURCES += main.cpp
这个上位机程序提供了以下功能:
- 串口通信:通过串口与STM32机器人通信
- 连接管理:自动检测可用串口,支持连接/断开操作
- 消毒控制:
- 设置消毒时长(1-120分钟)
- 开始/停止消毒
- 手动返回充电坞
- 状态监控:
- 实时显示机器人状态信息
- 消毒进度条显示
- 安全警告(检测到人体活动)
- 日志记录:所有操作和机器人反馈都有时间戳记录
通信协议说明:
START:时长- 开始消毒STOP- 停止消毒RETURN- 返回充电坞HUMAN_DETECTED- 机器人检测到人员DISINFECTION_COMPLETE- 消毒完成
模块代码设计
#include <stdint.h>
// 寄存器地址定义
#define RCC_BASE 0x40021000
#define GPIOA_BASE 0x40010800
#define GPIOB_BASE 0x40010C00
#define GPIOC_BASE 0x40011000
#define TIM2_BASE 0x40000000
#define TIM3_BASE 0x40000400
#define TIM4_BASE 0x40000800
#define NVIC_BASE 0xE000E100
#define EXTI_BASE 0x40010400
#define AFIO_BASE 0x40010000
// RCC 寄存器
#define RCC_APB2ENR *(volatile uint32_t *)(RCC_BASE + 0x18)
#define RCC_APB1ENR *(volatile uint32_t *)(RCC_BASE + 0x1C)
// GPIO 寄存器结构
typedef struct {
volatile uint32_t CRL;
volatile uint32_t CRH;
volatile uint32_t IDR;
volatile uint32_t ODR;
volatile uint32_t BSRR;
volatile uint32_t BRR;
volatile uint32_t LCKR;
} GPIO_TypeDef;
#define GPIOA ((GPIO_TypeDef *)GPIOA_BASE)
#define GPIOB ((GPIO_TypeDef *)GPIOB_BASE)
#define GPIOC ((GPIO_TypeDef *)GPIOC_BASE)
// 定时器寄存器结构
typedef struct {
volatile uint32_t CR1;
volatile uint32_t CR2;
volatile uint32_t SMCR;
volatile uint32_t DIER;
volatile uint32_t SR;
volatile uint32_t EGR;
volatile uint32_t CCMR1;
volatile uint32_t CCMR2;
volatile uint32_t CCER;
volatile uint32_t CNT;
volatile uint32_t PSC;
volatile uint32_t ARR;
volatile uint32_t CCR1;
volatile uint32_t CCR2;
volatile uint32_t CCR3;
volatile uint32_t CCR4;
} TIM_TypeDef;
#define TIM2 ((TIM_TypeDef *)TIM2_BASE)
#define TIM3 ((TIM_TypeDef *)TIM3_BASE)
#define TIM4 ((TIM_TypeDef *)TIM4_BASE)
// NVIC 寄存器
#define NVIC_ISER0 *(volatile uint32_t *)(NVIC_BASE + 0x00)
// EXTI 寄存器
#define EXTI_IMR *(volatile uint32_t *)(EXTI_BASE + 0x00)
#define EXTI_RTSR *(volatile uint32_t *)(EXTI_BASE + 0x08)
#define EXTI_FTSR *(volatile uint32_t *)(EXTI_BASE + 0x0C)
#define EXTI_PR *(volatile uint32_t *)(EXTI_BASE + 0x14)
// AFIO 寄存器
#define AFIO_EXTICR1 *(volatile uint32_t *)(AFIO_BASE + 0x08)
// 引脚定义
#define MOTOR_A_IN1_PIN 0 // PA0
#define MOTOR_A_IN2_PIN 1 // PA1
#define MOTOR_B_IN3_PIN 2 // PA2
#define MOTOR_B_IN4_PIN 3 // PA3
#define UV_RELAY_PIN 13 // PC13
#define HUMAN_IR_PIN 0 // PB0
#define DROP_IR_PIN 1 // PB1
#define ENCODER_A_PIN1 6 // PB6 (TIM4_CH1)
#define ENCODER_A_PIN2 7 // PB7 (TIM4_CH2)
// 全局变量
volatile uint8_t human_detected = 0;
volatile uint8_t drop_detected = 0;
volatile uint32_t disinfect_time_remaining = 0;
volatile uint8_t disinfect_active = 0;
// 函数声明
void SystemInit(void);
void GPIO_Init(void);
void TIM_Init(void);
void NVIC_Init(void);
void Motor_Control(uint8_t motor, int8_t speed);
void UV_Light_Control(uint8_t state);
uint8_t Read_Human_Sensor(void);
uint8_t Read_Drop_Sensor(void);
void Disinfect_Timer_Start(uint32_t duration);
void Disinfect_Timer_Stop(void);
void Move_Preset_Path(void);
void TIM2_IRQHandler(void) __attribute__((interrupt));
void EXTI0_IRQHandler(void) __attribute__((interrupt));
void EXTI1_IRQHandler(void) __attribute__((interrupt));
// 系统初始化
void SystemInit(void) {
// 启用时钟
RCC_APB2ENR |= (1 << 2) | (1 << 3) | (1 << 4) | (1 << 0); // GPIOA, GPIOB, GPIOC, AFIO
RCC_APB1ENR |= (1 << 0) | (1 << 1) | (1 << 2); // TIM2, TIM3, TIM4
}
// GPIO 初始化
void GPIO_Init(void) {
// GPIOA 配置: MOTOR_A_IN1, MOTOR_A_IN2, MOTOR_B_IN3, MOTOR_B_IN4 为推挽输出
GPIOA->CRL &= ~(0xFF << (MOTOR_A_IN1_PIN * 4)); // 清除配置
GPIOA->CRL |= (0x01 << (MOTOR_A_IN1_PIN * 4)) | (0x01 << (MOTOR_A_IN2_PIN * 4)) |
(0x01 << (MOTOR_B_IN3_PIN * 4)) | (0x01 << (MOTOR_B_IN4_PIN * 4)); // 推挽输出,50MHz
// GPIOB 配置: HUMAN_IR_PIN 和 DROP_IR_PIN 为输入上拉,ENCODER_A_PIN1 和 ENCODER_A_PIN2 为输入
GPIOB->CRL &= ~(0xFF << (HUMAN_IR_PIN * 4)); // 清除 HUMAN_IR_PIN 和 DROP_IR_PIN 配置
GPIOB->CRL |= (0x08 << (HUMAN_IR_PIN * 4)) | (0x08 << (DROP_IR_PIN * 4)); // 输入上拉
GPIOB->CRL &= ~(0xFF << (ENCODER_A_PIN1 * 4)); // 清除编码器引脚配置
GPIOB->CRL |= (0x04 << (ENCODER_A_PIN1 * 4)) | (0x04 << (ENCODER_A_PIN2 * 4)); // 输入浮空
// GPIOC 配置: UV_RELAY_PIN 为推挽输出
GPIOC->CRH &= ~(0xF << ((UV_RELAY_PIN - 8) * 4)); // 清除配置
GPIOC->CRH |= (0x01 << ((UV_RELAY_PIN - 8) * 4)); // 推挽输出,50MHz
// 设置上拉电阻 for HUMAN_IR_PIN and DROP_IR_PIN
GPIOB->ODR |= (1 << HUMAN_IR_PIN) | (1 << DROP_IR_PIN);
}
// 定时器初始化
void TIM_Init(void) {
// TIM3 用于电机 PWM
TIM3->PSC = 71; // 预分频器,72MHz / (71+1) = 1MHz
TIM3->ARR = 999; // 自动重载值,1MHz / 1000 = 1kHz PWM
TIM3->CCMR1 |= (0x06 << 4) | (0x06 << 12); // CH1 和 CH2 PWM 模式 1
TIM3->CCER |= (1 << 0) | (1 << 4); // 启用 CH1 和 CH2
TIM3->CR1 |= (1 << 0); // 启用 TIM3
// TIM4 用于编码器接口
TIM4->PSC = 0;
TIM4->ARR = 0xFFFF;
TIM4->SMCR |= (0x03 << 0); // 编码器模式 3
TIM4->CCER |= (1 << 0) | (1 << 4); // 启用 CH1 和 CH2
TIM4->CR1 |= (1 << 0); // 启用 TIM4
// TIM2 用于消毒定时
TIM2->PSC = 7199; // 72MHz / 7200 = 10kHz
TIM2->ARR = 10000; // 10kHz / 10000 = 1Hz, 1秒中断
TIM2->DIER |= (1 << 0); // 启用更新中断
TIM2->CR1 |= (1 << 0); // 启用 TIM2
}
// NVIC 初始化
void NVIC_Init(void) {
// 启用 TIM2 中断
NVIC_ISER0 |= (1 << 28); // TIM2 中断号 28
// 配置 EXTI 用于人体传感器和防跌落传感器
AFIO_EXTICR1 &= ~(0xFF << 0); // 清除 EXTI0 和 EXTI1 配置
AFIO_EXTICR1 |= (0x01 << 0) | (0x01 << 4); // EXTI0 和 EXTI1 连接到 PB0 和 PB1
EXTI_RTSR |= (1 << 0) | (1 << 1); // 上升沿触发
EXTI_FTSR |= (1 << 0) | (1 << 1); // 下降沿触发
EXTI_IMR |= (1 << 0) | (1 << 1); // 启用 EXTI0 和 EXTI1 中断
NVIC_ISER0 |= (1 << 6) | (1 << 7); // EXTI0 中断号 6, EXTI1 中断号 7
}
// 电机控制函数
void Motor_Control(uint8_t motor, int8_t speed) {
if (motor == 0) { // 电机 A
if (speed > 0) {
GPIOA->BSRR = (1 << MOTOR_A_IN1_PIN);
GPIOA->BRR = (1 << MOTOR_A_IN2_PIN);
} else if (speed < 0) {
GPIOA->BRR = (1 << MOTOR_A_IN1_PIN);
GPIOA->BSRR = (1 << MOTOR_A_IN2_PIN);
} else {
GPIOA->BRR = (1 << MOTOR_A_IN1_PIN) | (1 << MOTOR_A_IN2_PIN);
}
TIM3->CCR1 = (speed < 0) ? -speed : speed; // PWM 占空比
} else if (motor == 1) { // 电机 B
if (speed > 0) {
GPIOA->BSRR = (1 << MOTOR_B_IN3_PIN);
GPIOA->BRR = (1 << MOTOR_B_IN4_PIN);
} else if (speed < 0) {
GPIOA->BRR = (1 << MOTOR_B_IN3_PIN);
GPIOA->BSRR = (1 << MOTOR_B_IN4_PIN);
} else {
GPIOA->BRR = (1 << MOTOR_B_IN3_PIN) | (1 << MOTOR_B_IN4_PIN);
}
TIM3->CCR2 = (speed < 0) ? -speed : speed; // PWM 占空比
}
}
// 紫外灯控制
void UV_Light_Control(uint8_t state) {
if (state) {
GPIOC->BSRR = (1 << UV_RELAY_PIN); // 开启紫外灯
} else {
GPIOC->BRR = (1 << UV_RELAY_PIN); // 关闭紫外灯
}
}
// 读取人体传感器
uint8_t Read_Human_Sensor(void) {
return (GPIOB->IDR & (1 << HUMAN_IR_PIN)) ? 1 : 0;
}
// 读取防跌落传感器
uint8_t Read_Drop_Sensor(void) {
return (GPIOB->IDR & (1 << DROP_IR_PIN)) ? 1 : 0;
}
// 启动消毒定时器
void Disinfect_Timer_Start(uint32_t duration) {
disinfect_time_remaining = duration;
disinfect_active = 1;
TIM2->CNT = 0;
TIM2->CR1 |= (1 << 0); // 启用 TIM2
}
// 停止消毒定时器
void Disinfect_Timer_Stop(void) {
disinfect_active = 0;
TIM2->CR1 &= ~(1 << 0); // 禁用 TIM2
UV_Light_Control(0); // 关闭紫外灯
// 返回充电坞逻辑可在此添加
}
// 预设路径移动
void Move_Preset_Path(void) {
// 示例路径: 前进 5 秒,左转 2 秒,前进 5 秒
Motor_Control(0, 80); // 电机 A 前进
Motor_Control(1, 80); // 电机 B 前进
for (volatile int i = 0; i < 5000000; i++); // 简单延时
Motor_Control(0, -80); // 电机 A 后退
Motor_Control(1, 80); // 电机 B 前进,左转
for (volatile int i = 0; i < 2000000; i++);
Motor_Control(0, 80);
Motor_Control(1, 80);
for (volatile int i = 0; i < 5000000; i++);
Motor_Control(0, 0); // 停止
Motor_Control(1, 0);
}
// TIM2 中断服务程序
void TIM2_IRQHandler(void) {
if (TIM2->SR & (1 << 0)) { // 更新中断
TIM2->SR &= ~(1 << 0); // 清除中断标志
if (disinfect_active && disinfect_time_remaining > 0) {
disinfect_time_remaining--;
if (disinfect_time_remaining == 0) {
Disinfect_Timer_Stop();
}
}
}
}
// EXTI0 中断服务程序 (人体传感器)
void EXTI0_IRQHandler(void) {
if (EXTI_PR & (1 << 0)) {
EXTI_PR |= (1 << 0); // 清除中断标志
human_detected = Read_Human_Sensor();
if (human_detected) {
UV_Light_Control(0); // 检测到人,立即关闭紫外灯
}
}
}
// EXTI1 中断服务程序 (防跌落传感器)
void EXTI1_IRQHandler(void) {
if (EXTI_PR & (1 << 1)) {
EXTI_PR |= (1 << 1); // 清除中断标志
drop_detected = Read_Drop_Sensor();
if (drop_detected) {
Motor_Control(0, 0); // 停止电机
Motor_Control(1, 0);
}
}
}
// 主函数
int main(void) {
SystemInit();
GPIO_Init();
TIM_Init();
NVIC_Init();
// 主循环
while (1) {
// 检查传感器状态
human_detected = Read_Human_Sensor();
drop_detected = Read_Drop_Sensor();
// 无人且无跌落时启动消毒
if (!human_detected && !drop_detected && !disinfect_active) {
Disinfect_Timer_Start(300); // 设置消毒时长为 300 秒(5 分钟)
UV_Light_Control(1);
Move_Preset_Path(); // 开始移动
}
// 如果消毒激活,检查是否结束
if (disinfect_active && disinfect_time_remaining == 0) {
Disinfect_Timer_Stop();
}
// 防跌落处理
if (drop_detected) {
Motor_Control(0, 0);
Motor_Control(1, 0);
}
}
}
项目核心代码
#include "stm32f10x.h"
// 引脚定义
#define MOTOR_IN1_PIN GPIO_Pin_0
#define MOTOR_IN2_PIN GPIO_Pin_1
#define MOTOR_IN3_PIN GPIO_Pin_2
#define MOTOR_IN4_PIN GPIO_Pin_3
#define MOTOR_PORT GPIOA
#define UV_RELAY_PIN GPIO_Pin_4
#define UV_PORT GPIOA
#define HUMAN_SENSOR_PIN GPIO_Pin_5
#define HUMAN_PORT GPIOA
#define DROP_SENSOR_PIN GPIO_Pin_6
#define DROP_PORT GPIOA
// 全局变量
volatile uint32_t uv_timer_count = 0;
volatile uint8_t uv_enabled = 0;
volatile uint8_t human_detected = 0;
volatile uint8_t drop_detected = 0;
// 函数声明
void SystemInit(void);
void GPIO_Configuration(void);
void TIM2_Configuration(void);
void NVIC_Configuration(void);
void Motor_Forward(void);
void Motor_Backward(void);
void Motor_Stop(void);
void Motor_TurnLeft(void);
void Motor_TurnRight(void);
void UV_Light_On(void);
void UV_Light_Off(void);
uint8_t Read_Human_Sensor(void);
uint8_t Read_Drop_Sensor(void);
void Delay_ms(uint32_t ms);
int main(void)
{
SystemInit();
GPIO_Configuration();
TIM2_Configuration();
NVIC_Configuration();
// 预设消毒时间(30分钟)
uint32_t preset_time = 30 * 60; // 30分钟转换为秒
while(1)
{
// 读取传感器状态
human_detected = Read_Human_Sensor();
drop_detected = Read_Drop_Sensor();
// 安全条件检查:无人且无跌落风险
if(!human_detected && !drop_detected)
{
if(!uv_enabled)
{
// 开始消毒
UV_Light_On();
uv_enabled = 1;
uv_timer_count = preset_time;
TIM_Cmd(TIM2, ENABLE); // 启动定时器
}
// 按照预设路径移动
Motor_Forward();
Delay_ms(2000);
Motor_TurnRight();
Delay_ms(1000);
Motor_Forward();
Delay_ms(2000);
Motor_TurnLeft();
Delay_ms(1000);
}
else
{
// 安全条件不满足,停止消毒和移动
if(uv_enabled)
{
UV_Light_Off();
uv_enabled = 0;
TIM_Cmd(TIM2, DISABLE); // 停止定时器
}
Motor_Stop();
// 等待安全条件恢复
while(human_detected || drop_detected)
{
human_detected = Read_Human_Sensor();
drop_detected = Read_Drop_Sensor();
Delay_ms(100);
}
}
// 检查消毒时间是否结束
if(uv_timer_count == 0 && uv_enabled)
{
UV_Light_Off();
uv_enabled = 0;
TIM_Cmd(TIM2, DISABLE);
// 返回充电坞
Motor_Backward();
Delay_ms(5000); // 假设返回充电坞需要5秒
Motor_Stop();
// 重置定时器
uv_timer_count = preset_time;
}
Delay_ms(100);
}
}
void SystemInit(void)
{
// 启用时钟
RCC->APB2ENR |= RCC_APB2ENR_IOPAEN | RCC_APB2ENR_IOPBEN;
RCC->APB1ENR |= RCC_APB1ENR_TIM2EN;
// 设置系统时钟
SystemCoreClockUpdate();
}
void GPIO_Configuration(void)
{
GPIO_InitTypeDef GPIO_InitStructure;
// 电机控制引脚配置为推挽输出
GPIO_InitStructure.GPIO_Pin = MOTOR_IN1_PIN | MOTOR_IN2_PIN |
MOTOR_IN3_PIN | MOTOR_IN4_PIN | UV_RELAY_PIN;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(MOTOR_PORT, &GPIO_InitStructure);
// 传感器引脚配置为输入
GPIO_InitStructure.GPIO_Pin = HUMAN_SENSOR_PIN | DROP_SENSOR_PIN;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
GPIO_Init(HUMAN_PORT, &GPIO_InitStructure);
}
void TIM2_Configuration(void)
{
// 定时器2配置为1秒中断
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
TIM_TimeBaseStructure.TIM_Period = 1000 - 1; // 1秒
TIM_TimeBaseStructure.TIM_Prescaler = 7200 - 1; // 72MHz/7200 = 10kHz
TIM_TimeBaseStructure.TIM_ClockDivision = 0;
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
TIM_TimeBaseInit(TIM2, &TIM_TimeBaseStructure);
TIM_ITConfig(TIM2, TIM_IT_Update, ENABLE);
TIM_Cmd(TIM2, DISABLE);
}
void NVIC_Configuration(void)
{
NVIC_InitTypeDef NVIC_InitStructure;
NVIC_InitStructure.NVIC_IRQChannel = TIM2_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
}
void TIM2_IRQHandler(void)
{
if(TIM_GetITStatus(TIM2, TIM_IT_Update) != RESET)
{
TIM_ClearITPendingBit(TIM2, TIM_IT_Update);
if(uv_enabled && uv_timer_count > 0)
{
uv_timer_count--;
}
}
}
void Motor_Forward(void)
{
MOTOR_PORT->BSRR = MOTOR_IN1_PIN; // IN1高
MOTOR_PORT->BRR = MOTOR_IN2_PIN; // IN2低
MOTOR_PORT->BSRR = MOTOR_IN3_PIN; // IN3高
MOTOR_PORT->BRR = MOTOR_IN4_PIN; // IN4低
}
void Motor_Backward(void)
{
MOTOR_PORT->BRR = MOTOR_IN1_PIN; // IN1低
MOTOR_PORT->BSRR = MOTOR_IN2_PIN; // IN2高
MOTOR_PORT->BRR = MOTOR_IN3_PIN; // IN3低
MOTOR_PORT->BSRR = MOTOR_IN4_PIN; // IN4高
}
void Motor_Stop(void)
{
MOTOR_PORT->BRR = MOTOR_IN1_PIN | MOTOR_IN2_PIN |
MOTOR_IN3_PIN | MOTOR_IN4_PIN;
}
void Motor_TurnLeft(void)
{
MOTOR_PORT->BRR = MOTOR_IN1_PIN; // IN1低
MOTOR_PORT->BRR = MOTOR_IN2_PIN; // IN2低
MOTOR_PORT->BSRR = MOTOR_IN3_PIN; // IN3高
MOTOR_PORT->BRR = MOTOR_IN4_PIN; // IN4低
}
void Motor_TurnRight(void)
{
MOTOR_PORT->BSRR = MOTOR_IN1_PIN; // IN1高
MOTOR_PORT->BRR = MOTOR_IN2_PIN; // IN2低
MOTOR_PORT->BRR = MOTOR_IN3_PIN; // IN3低
MOTOR_PORT->BRR = MOTOR_IN4_PIN; // IN4低
}
void UV_Light_On(void)
{
UV_PORT->BSRR = UV_RELAY_PIN;
}
void UV_Light_Off(void)
{
UV_PORT->BRR = UV_RELAY_PIN;
}
uint8_t Read_Human_Sensor(void)
{
return (HUMAN_PORT->IDR & HUMAN_SENSOR_PIN) ? 1 : 0;
}
uint8_t Read_Drop_Sensor(void)
{
return (DROP_PORT->IDR & DROP_SENSOR_PIN) ? 1 : 0;
}
void Delay_ms(uint32_t ms)
{
volatile uint32_t i, j;
for(i = 0; i < ms; i++)
for(j = 0; j < 7200; j++);
}
总结
实验室紫外灯智能消毒机器人是一种高效且安全的自动化设备,专为实验室内环境消毒设计。它通过预设路径自主移动,确保全面覆盖消毒区域,同时利用红外人体感应模块实时监测环境,仅在确认无人状态下才启动紫外灯消毒,有效避免对人体造成伤害。
该机器人具备灵活的定时功能,用户可自定义消毒时长,任务完成后自动返回充电坞并关闭紫外灯,实现全流程自动化管理。此外,防跌落功能通过红外对管传感器实时检测边缘障碍,保障机器人在运行过程中的稳定与安全。
硬件系统以STM32F103C8T6单片机为核心控制器,协调各模块工作。移动底盘采用带编码器的直流减速电机与L298N驱动板,确保精准路径跟踪;核心执行器包括紫外灯管和继电器模块,负责消毒操作;安全传感部分集成HC-SR501人体红外传感器和红外对管防跌落传感器,共同构建多层次防护机制。整体设计兼顾功能性与可靠性,适用于各类实验室场景。
- 点赞
- 收藏
- 关注作者
评论(0)