arduino/舵机/sketch_jun16b.ino

#include <Servo.h>
#include <ros.h>
#include "std_msgs/String.h"
#include "std_msgs/UInt8.h"
#include "std_msgs/Bool.h"

#define SERVO_PIN_1 3
#define SERVO_PIN_2 6
#define SERVO_PIN_3 9
#define SERVO_PIN_4 10
#define SERVO_DEFAULT 90

// 普通舵机：OPEN=0, CLOSE=90
#define SERVO_OPEN    0
#define SERVO_CLOSE   90

// 舵机2反向：OPEN=90, CLOSE=0
#define SERVO_OPEN_2  180
#define SERVO_CLOSE_2 90

// ============================================
// 舵机任务类（非阻塞状态机，自带完美运行态防抖）
// ============================================
class ServoTask {
public:
  enum State { IDLE, OPENING, WAIT_OPEN, CLOSING, WAIT_CLOSE };
  
  ServoTask() : state_(IDLE), startTime_(0), active_(false), completed_(false), reversed_(false) {}
  
  void attach(int pin) { servo_.attach(pin); }
  
  // 🌟 设置是否反向
  void setReversed(bool rev) { reversed_ = rev; }
  
  void trigger() {
    if (active_) return;
    active_ = true;
    completed_ = false;
    state_ = OPENING;
  }
  
  void setImmediate(int angle) {
    active_ = false;
    completed_ = false;
    state_ = IDLE;
    servo_.write(angle);
  }
  
  bool checkAndClearCompleted() { 
    if (completed_) {
      completed_ = false;
      return true;
    }
    return false;
  }
  
  void update() {
    if (!active_) return;
    
    unsigned long now = millis();
    switch (state_) {
      case OPENING:
        // 🌟 根据是否反向选择 OPEN 角度
        servo_.write(reversed_ ? SERVO_OPEN_2 : SERVO_OPEN);
        state_ = WAIT_OPEN;
        startTime_ = now;
        break;
        
      case WAIT_OPEN:
        if (now - startTime_ >= 1000) state_ = CLOSING;
        break;
        
      case CLOSING:
        // 🌟 根据是否反向选择 CLOSE 角度
        servo_.write(reversed_ ? SERVO_CLOSE_2 : SERVO_CLOSE);
        state_ = WAIT_CLOSE;
        startTime_ = now;
        break;
        
      case WAIT_CLOSE:
        if (now - startTime_ >= 1000) {
          state_ = IDLE;
          active_ = false;
          completed_ = true;
        }
        break;
        
      default: break;
    }
  }
  
private:
  Servo servo_;
  State state_;
  unsigned long startTime_;
  bool active_;
  bool completed_;
  bool reversed_;  // 🌟 是否反向
};

// ============================================
// 全局对象与ROS通信
// ============================================
ros::NodeHandle nh;

ServoTask task1, task2, task3, task4;
ServoTask* tasks[4] = {&task1, &task2, &task3, &task4};

std_msgs::Bool dropStatusMsg;
std_msgs::UInt8 dropDoneMsg;

ros::Publisher pubDropStatus("/payload_drop_status", &dropStatusMsg);
ros::Publisher pubDropDone("/payload_drop_done", &dropDoneMsg);

// ============================================
// 执行投放掩码
// ============================================
void executeDrop(uint8_t servoMask) {
  for (int i = 0; i < 4; i++) {
    if (servoMask & (1 << i)) tasks[i]->trigger();
  }
}

// ============================================
// 回调核心：去掉了外部延时锁，指令立刻响应
// ============================================
void stringCallback(const std_msgs::String& msg) {
  String cmd = msg.data;
  
  if      (cmd == "servo_1")      executeDrop(0x01);
  else if (cmd == "servo_2")      executeDrop(0x02);
  else if (cmd == "servo_3")      executeDrop(0x04);
  else if (cmd == "servo_4")      executeDrop(0x08);
  else if (cmd == "servo_12")     executeDrop(0x03);
  else if (cmd == "servo_34")     executeDrop(0x0C);
  else if (cmd == "servo_1234")   executeDrop(0x0F);
  
  // 强行中断并复位
  else if (cmd == "servo_1234_0") for (int i=0; i<4; i++) tasks[i]->setImmediate(SERVO_OPEN);
  else if (cmd == "servo_1234_1") for (int i=0; i<4; i++) tasks[i]->setImmediate(SERVO_CLOSE);
  else if (cmd == "servo_reset")  for (int i=0; i<4; i++) tasks[i]->setImmediate(SERVO_DEFAULT);
}

ros::Subscriber<std_msgs::String> subTest("/arduino_ros", &stringCallback);
ros::Subscriber<std_msgs::String> subVision("/payload_drop_cmd", &stringCallback);

// ============================================
// Arduino 主循环
// ============================================
void setup() {
  task1.attach(SERVO_PIN_1);
  task2.attach(SERVO_PIN_2);
  task3.attach(SERVO_PIN_3);
  task4.attach(SERVO_PIN_4);
  
  // 🌟 设置舵机2反向
  task2.setReversed(true);
  task4.setReversed(true);
  
  for (int i = 0; i < 4; i++) tasks[i]->setImmediate(SERVO_DEFAULT);
  
  nh.initNode();
  nh.advertise(pubDropStatus);
  nh.advertise(pubDropDone);
  nh.subscribe(subTest);
  nh.subscribe(subVision);
}

void loop() {
  // 1. 更新每个舵机的动作状态
  for (int i = 0; i < 4; i++) {
    tasks[i]->update();
    
    // 2. 如果动作刚刚完成，发送 ROS 提醒
    if (tasks[i]->checkAndClearCompleted()) {
      dropStatusMsg.data = true;
      pubDropStatus.publish(&dropStatusMsg);
      dropDoneMsg.data = i + 1;
      pubDropDone.publish(&dropDoneMsg);
    }
  }
  
  nh.spinOnce();
  delay(10);
}