/*!
 * Gripper Fine-Tuning (Pin 14 Fixed)
 * 启动位置: 97, 85, 138, 125
 * 硬件定义:
 * - Base: Pin 41
 * - Big Arm: Pin 43 & 45 (补偿 -5)
 * - Mid Arm: Pin 47 & 49 (补偿 +4)
 * - Gripper: Pin 14 (已修正) <--- 关键修改
 */
#include <DFRobot_Servo.h>

Servo sv_base;      // 41
Servo sv_arm_main;  // 43
Servo sv_arm_sub;   // 45
Servo sv_mid_main;  // 47
Servo sv_mid_sub;   // 49
Servo sv_grip;      // 14

// 初始位置 [Base, Big, Mid, Grip]
// 对应 ID 1, 2, 3, 4
int currentLogicPos[5] = {0, 97, 85, 138, 125}; 

const int SPEED_DELAY = 30; 

// --- 核心更新函数 ---
void updateAllServos() {
  // 1. Base (41)
  sv_base.angle(currentLogicPos[1]);
  
  // 2. Big Arm (43 & 45)
  int bigAngle = currentLogicPos[2];
  sv_arm_main.angle(bigAngle);
  sv_arm_sub.angle(180 - bigAngle - 5); // 补偿
  
  // 3. Mid Arm (47 & 49)
  int midAngle = currentLogicPos[3];
  sv_mid_main.angle(midAngle + 4);      // 补偿
  sv_mid_sub.angle(180 - midAngle);
  
  // 4. Gripper (14)
  sv_grip.angle(currentLogicPos[4]);
}

// 平滑移动
void moveSmooth(int id, int target) {
  if (target < 0) target = 0;
  if (target > 180) target = 180;
  
  int current = currentLogicPos[id];
  
  while (current != target) {
    if (current < target) current++;
    else current--;
    
    currentLogicPos[id] = current;
    updateAllServos();
    delay(SPEED_DELAY);
  }
}

void setup() {
  Serial.begin(9600);
  
  // --- 绑定引脚 ---
  sv_base.attach(41);
  sv_arm_main.attach(43);
  sv_arm_sub.attach(45);
  sv_mid_main.attach(47);
  sv_mid_sub.attach(49);
  
  // [关键修改] 夹爪绑定到 14 号引脚
  sv_grip.attach(14); 
  
  // 上电直接执行初始位置
  updateAllServos();
  
  Serial.println("System Ready. Gripper on Pin 14.");
}

void loop() {
  if (Serial.available() > 0) {
    String input = Serial.readStringUntil('\n');
    int colonIndex = input.indexOf(':');
    
    if (colonIndex != -1) {
      int id = input.substring(0, colonIndex).toInt();
      int angle = input.substring(colonIndex + 1).toInt();
      
      if (id >= 1 && id <= 4) {
        moveSmooth(id, angle);
      }
    }
  }
}