#include <Servo.h>  // 使用内置舵机库

// 定义三个舵机对象
Servo servo_arm;     // 机械臂上下转动
Servo servo_rotate;  // 机械爪左右转动
Servo servo_grip;    // 爪子开合

// 初始角度设置（需根据实际机械结构调整）
#define ARM_INIT 0       // 机械臂初始水平位置
#define ROTATE_INIT 150  // 机械爪初始朝前位置
#define GRIP_OPEN 30     // 爪子张开角度（小角度）
#define GRIP_CLOSE 140   // 爪子闭合角度（大角度）

void setup() {
  Serial.begin(9600);
  // 绑定舵机到对应引脚
  servo_arm.attach(8);     // 机械臂舵机接Pin8
  servo_rotate.attach(9);  // 旋转舵机接Pin9
  servo_grip.attach(10);   // 爪子舵机接Pin10

  // 初始化位置
  servo_arm.write(60);
  servo_rotate.write(ROTATE_INIT);
  servo_grip.write(GRIP_OPEN);
  delay(1000);  // 等待舵机到位
  Serial.println("机械爪初始化完成！");
}

void loop() {
  // 示例动作序列：抓取物体
  gripObject();  // 执行抓取
  delay(1000);   // 保持3秒
  //releaseObject();   // 释放物体
  // delay(3000);       // 等待3秒后重复
}

// 抓取物体动作函数
void gripObject() {
  servo_arm.write(180);  // 机械臂下移（角度减小）
  delay(1000);

  servo_rotate.write(90);
  delay(800);
  servo_grip.write(GRIP_OPEN);  // 张开爪子
  delay(1000);
  servo_grip.write(GRIP_CLOSE);  // 闭合爪子
  delay(1000);
  servo_arm.write(ARM_INIT);  // 抬升机械臂
}

// 释放物体动作函数
void releaseObject() {
  servo_arm.write(180);  // 机械臂下移（角度减小）
  delay(1000);

  servo_rotate.write(110);
  delay(800);
  servo_grip.write(GRIP_OPEN);  // 张开爪子
  delay(1000);
  servo_grip.write(GRIP_CLOSE);  // 闭合爪子
  delay(1000);
  servo_arm.write(ARM_INIT);  // 抬升机械臂
}