#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>

Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();

// 舵机PWM范围
#define SERVOMIN  102
#define SERVOMAX  512

// 舵机编号（保留原始定义）
#define LF 0   // 左前腿 Left Front
#define RF 1   // 右前腿 Right Front
#define RH 2   // 右后腿 Right Hind
#define LH 3   // 左后腿 Left Hind

// 角度定义
#define NEUTRAL       90  // 中立位置
#define STEP_UP       65  // 抬腿角度
#define STEP_FORWARD  40  // 向前伸展角度
#define STEP_BACK     120 // 向后推进角度
#define STEP_UP1      115  // 抬腿角度
#define STEP_FORWARD1 140  // 向前伸展角度
#define STEP_BACK1     60

// 定义步态延迟(毫秒)
#define MOVEMENT_DELAY 150

void setup() {
  Serial.begin(9600);
  Serial.println("四足机器狗启动中...");
  
  pwm.begin();
  pwm.setPWMFreq(50);  // 舵机标准50Hz更新频率
  
  delay(500);
  
  // 初始化所有腿到中立位置
  allLegsToNeutral();
  delay(1000);
}

// 将角度转换为PWM值，考虑舵机方向
int angleToPulse(int angle, boolean isReversed) {
  if (isReversed) {
    angle = 180 - angle;
  }
  return map(angle, 0, 180, SERVOMIN, SERVOMAX);
}

// 移动单个舵机到指定角度
void moveLeg(uint8_t legId, int angle) {
  boolean isReversed = (legId == RF || legId == LH);
  int pulse = angleToPulse(angle, isReversed);
  pwm.setPWM(legId, 0, pulse);
}

// 所有腿回到中立位置
void allLegsToNeutral() {
  moveLeg(LF, NEUTRAL);
  moveLeg(RF, NEUTRAL);
  moveLeg(RH, NEUTRAL);
  moveLeg(LH, NEUTRAL);
}

// 对角线步态行走（更稳定）
void walkDiagonal() {
  // 第一组：左前腿和右后腿
  // 抬起
  moveLeg(LF, STEP_UP);
  moveLeg(RH, STEP_UP);
  delay(MOVEMENT_DELAY);
  
  // 前伸
  moveLeg(LF, STEP_FORWARD);
  moveLeg(RH, STEP_FORWARD);
  delay(MOVEMENT_DELAY);
  
  // 放下
  moveLeg(LF, NEUTRAL);
  moveLeg(RH, NEUTRAL);
  delay(MOVEMENT_DELAY);
  
  // 同时，后蹬推进
  moveLeg(RF, STEP_BACK);
  moveLeg(LH, STEP_BACK);
  delay(MOVEMENT_DELAY * 2);
  
  // 第二组：右前腿和左后腿
  // 抬起
  moveLeg(RF, STEP_UP);
  moveLeg(LH, STEP_UP);
  delay(MOVEMENT_DELAY);
  
  // 前伸
  moveLeg(RF, STEP_FORWARD);
  moveLeg(LH, STEP_FORWARD);
  delay(MOVEMENT_DELAY);
  
  // 放下
  moveLeg(RF, NEUTRAL);
  moveLeg(LH, NEUTRAL);
  delay(MOVEMENT_DELAY);
  
  // 同时，后蹬推进
  moveLeg(LF, STEP_BACK);
  moveLeg(RH, STEP_BACK);
  delay(MOVEMENT_DELAY * 2);
  
  // 所有腿回到中立位置，准备下一个循环
  allLegsToNeutral();
  delay(MOVEMENT_DELAY);
}

// 爬行步态（一次一条腿移动，更稳定但较慢）
void crawl() {
  // 移动顺序：LF -> RH -> RF -> LH

  // 左前腿(LF)
  moveLeg(LF, STEP_UP);
  delay(MOVEMENT_DELAY);
  moveLeg(LF, STEP_FORWARD);
  delay(MOVEMENT_DELAY);
  moveLeg(LF, NEUTRAL);
  delay(MOVEMENT_DELAY);

  // 右后腿(RH)
  moveLeg(RH, STEP_UP1);
  delay(MOVEMENT_DELAY);
  moveLeg(RH, STEP_FORWARD1);
  delay(MOVEMENT_DELAY);
  moveLeg(RH, NEUTRAL);
  delay(MOVEMENT_DELAY);

  // 右前腿(RF)
  moveLeg(RF, STEP_UP);
  delay(MOVEMENT_DELAY);
  moveLeg(RF, STEP_FORWARD);
  delay(MOVEMENT_DELAY);
  moveLeg(RF, NEUTRAL);
  delay(MOVEMENT_DELAY);

  // 左后腿(LH)
  moveLeg(LH, STEP_UP1);
  delay(MOVEMENT_DELAY);
  moveLeg(LH, STEP_FORWARD1);
  delay(MOVEMENT_DELAY);
  moveLeg(LH, NEUTRAL);
  delay(MOVEMENT_DELAY);
}

// 摇尾巴动作（所有腿微微左右移动）
void wagTail() {
  for (int i = 0; i < 3; i++) {
    // 轻微左右摆动
    moveLeg(LF, NEUTRAL - 15);
    moveLeg(RF, NEUTRAL - 15);
    moveLeg(RH, NEUTRAL - 15);
    moveLeg(LH, NEUTRAL - 15);
    delay(200);
    
    moveLeg(LF, NEUTRAL + 15);
    moveLeg(RF, NEUTRAL + 15);
    moveLeg(RH, NEUTRAL + 15);
    moveLeg(LH, NEUTRAL + 15);
    delay(200);
  }
  
  allLegsToNeutral();
  delay(200);
}
// 趴下动作
void lieDown() {
  Serial.println("正在趴下...");

  // 将所有腿移动到较低的位置并向后收缩
  moveLeg(LF, 50);   // 左前腿向后收 (根据你的机器人调整)
  moveLeg(RF, 50);  // 右前腿向后收 (根据你的机器人调整)
  moveLeg(RH, 50);  // 右后腿向后收 (根据你的机器人调整)
  moveLeg(LH, 50);   // 左后腿向后收 (根据你的机器人调整)

  delay(500); // 暂停一段时间

  // 进一步降低身体
  moveLeg(LF, 30);
  moveLeg(RF, 30);
  moveLeg(RH, 30);
  moveLeg(LH, 30);

  delay(500);

  Serial.println("趴下完成。");
}

// 站起来动作 (恢复到中立位置)
void standUp() {
  Serial.println("正在站起来...");
  allLegsToNeutral();
  delay(500);
  Serial.println("站起来完成。");
}

void loop() {
  // 使用对角线步态行走
  //walkDiagonal();
  
  // 每走5步，做一次摇尾巴动作
  //static int stepCount = 0;
  //stepCount++;
  
  //if (stepCount >= 5) {
  //wagTail();
  lieDown();
  standUp();
   // stepCount = 0;
 // }
  
  // 如果要使用爬行步态，取消下面注释并注释掉上面的walkDiagonal()
  //crawl();
}