arduino代码

2026-07-18 18:22:58 +08:00
parent 2b7a320983
commit a2825ed71a
438 changed files with 36538 additions and 0 deletions
@@ -0,0 +1,185 @@
+#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);
+}