104 lines
2.5 KiB
Arduino
104 lines
2.5 KiB
Arduino
#include <Servo.h>
|
|
#include <ros.h>
|
|
#include "std_msgs/String.h"
|
|
|
|
#define LASER_PIN 3 // 激光笔 3号口
|
|
#define SERVO_PIN 5 // 舵机 5号口
|
|
#define SERVO_DEFAULT 0 // 默认角度：0度
|
|
#define SERVO_ACTIVE 70 // 触发角度：70度
|
|
|
|
Servo myServo;
|
|
ros::NodeHandle nh;
|
|
|
|
// 1. 激光自动复位定时器
|
|
bool isLaserAuto = false;
|
|
unsigned long laserStartTime = 0;
|
|
|
|
// 2. 舵机自动复位定时器
|
|
bool isServoAuto = false;
|
|
unsigned long servoStartTime = 0;
|
|
|
|
// ============================================
|
|
// 1. 激光控制回调 (target_recognition_rusult)
|
|
// ============================================
|
|
void laserCb(const std_msgs::String& msg) {
|
|
String cmd = String(msg.data);
|
|
cmd.toLowerCase();
|
|
|
|
if (cmd == "yes") {
|
|
// 自动模式：亮激光，启动 2 秒定时
|
|
digitalWrite(LASER_PIN, HIGH);
|
|
isLaserAuto = true;
|
|
laserStartTime = millis();
|
|
}
|
|
else if (cmd == "laser_on") {
|
|
// 手动常亮
|
|
digitalWrite(LASER_PIN, HIGH);
|
|
isLaserAuto = false;
|
|
}
|
|
else if (cmd == "off") {
|
|
// 手动关闭
|
|
digitalWrite(LASER_PIN, LOW);
|
|
isLaserAuto = false;
|
|
}
|
|
}
|
|
|
|
// ============================================
|
|
// 2. 舵机控制回调 (/payload_drop_cmd)
|
|
// ============================================
|
|
void servoCb(const std_msgs::String& msg) {
|
|
String cmd = String(msg.data);
|
|
cmd.toLowerCase();
|
|
|
|
if (cmd == "servo_1") {
|
|
// 自动模式：转动 90 度，启动 2 秒定时
|
|
myServo.write(SERVO_ACTIVE);
|
|
isServoAuto = true;
|
|
servoStartTime = millis();
|
|
}
|
|
else if (cmd == "servo_on") {
|
|
// 手动保持 90 度
|
|
myServo.write(SERVO_ACTIVE);
|
|
isServoAuto = false;
|
|
}
|
|
else if (cmd == "servo_off") {
|
|
// 手动回到 0 度
|
|
myServo.write(SERVO_DEFAULT);
|
|
isServoAuto = false;
|
|
}
|
|
}
|
|
|
|
ros::Subscriber<std_msgs::String> subTarget("target_recognition_rusult", &laserCb);
|
|
ros::Subscriber<std_msgs::String> subVision("/payload_drop_cmd", &servoCb);
|
|
|
|
void setup() {
|
|
pinMode(LASER_PIN, OUTPUT);
|
|
digitalWrite(LASER_PIN, LOW); // 默认关闭激光
|
|
|
|
myServo.attach(SERVO_PIN);
|
|
myServo.write(SERVO_DEFAULT); // 默认舵机归零
|
|
|
|
nh.initNode();
|
|
nh.subscribe(subTarget);
|
|
nh.subscribe(subVision);
|
|
}
|
|
|
|
void loop() {
|
|
unsigned long now = millis();
|
|
|
|
// 激光 2 秒自动复位
|
|
if (isLaserAuto && (now - laserStartTime >= 2000)) {
|
|
digitalWrite(LASER_PIN, LOW);
|
|
isLaserAuto = false;
|
|
}
|
|
|
|
// 舵机 2 秒自动复位
|
|
if (isServoAuto && (now - servoStartTime >= 2000)) {
|
|
myServo.write(SERVO_DEFAULT);
|
|
isServoAuto = false;
|
|
}
|
|
|
|
nh.spinOnce();
|
|
delay(1);
|
|
}
|