#include <Servo.h>  // 引入舵机库

// 定义舵机对象
Servo servoLeft;      // 左夹爪舵机（上），接 D5
Servo servoRight;     // 右夹爪舵机（中），接 D6
Servo servoRotate;    // 旋转舵机（下）（塔吊旋转），接 D4

// 超声波模块1（左）（检测纸团） → TRIG 接 D7，ECHO 接 D8
const int trig1 = 7;   // TRIG 引脚，检测纸团，D7
const int echo1 = 8;   // ECHO 引脚，检测纸团，D8

// 超声波模块2（右）（检测桶是否满） → TRIG 接 D9，ECHO 接 D10
const int trig2 = 9;   // TRIG 引脚，检测桶是否满，D9
const int echo2 = 10;  // ECHO 引脚，检测桶是否满，D10

// LED 灯（提示桶满） → LED 接 D11
const int ledPin = 11;  // LED 提示灯引脚，D11

// 按钮检测纸团（模拟传感器） → 按钮接 D12
const int buttonPin = 12; // 按钮检测是否有纸团，D12

enum State {
    SEARCHING,
    PAUSE_BEFORE_GRABBING,
    GRABBING,
    GRABBING_CHECK,
    ROTATING,
    PAUSE_BEFORE_DROPPING,
    DROPPING,
    RETURNING
};

State currentState = SEARCHING;

unsigned long stateStartTime = 0;
 
bool lastButtonState = HIGH;   
bool buttonPressed = false;   
int lastFoundAngle = 0;


void setup() {
  // 初始化舵机
  servoLeft.attach(5);    // 左夹爪接 D5
  servoRight.attach(6);   // 右夹爪接 D6
  servoRotate.attach(4);  // 塔吊旋转接 D4

  // 初始化超声波模块引脚
  pinMode(trig1, OUTPUT);
  pinMode(echo1, INPUT);
  pinMode(trig2, OUTPUT);
  pinMode(echo2, INPUT);

  // 初始化 LED 引脚
  pinMode(ledPin, OUTPUT);

  pinMode(buttonPin, INPUT_PULLUP);   
  Serial.begin(9600);

}

long readDistanceCM(int trigPin, int echoPin) {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  long duration = pulseIn(echoPin, HIGH);   
  long distance = duration * 0.034 / 2;  
  return distance;
}

unsigned long lastPrintTime = 0;
const unsigned long printInterval = 1000;
int currentAngle = 0;

void slowMove(Servo &servo, int startAngle, int endAngle, int stepDelay) {
    if (startAngle < endAngle) {
        for (int pos = startAngle; pos <= endAngle; pos++) {
            servo.write(pos);
            delay(stepDelay);
        }
    } else {
        for (int pos = startAngle; pos >= endAngle; pos--) {
            servo.write(pos);
            delay(stepDelay);
        }
    }
}

void loop() {
  long distance2 = readDistanceCM(trig2, echo2);   
  bool currentButtonState = digitalRead(buttonPin);
  if (lastButtonState == HIGH && currentButtonState == LOW) {
      buttonPressed = true;
  }
lastButtonState = currentButtonState;


  if (distance2 < 20) { 
        for (int i = 0; i < 3; i++) {
            digitalWrite(ledPin, HIGH);
            delay(300);
            digitalWrite(ledPin, LOW);
            delay(300);
        }
        digitalWrite(ledPin, HIGH);
        Serial.println("桶已满，程序停止。");
        while (true); 
  }
  
  switch (currentState) {
        case SEARCHING:
            servoRotate.write(currentAngle);
            delay(40);
            currentAngle = (currentAngle + 1) % 180;

            if (millis() % 500 < 10) {   
                Serial.println("检测中---");
                delay(20);
            }

            if (buttonPressed) {
                Serial.println("检测到纸团，准备进入夹取阶段");
                lastFoundAngle = currentAngle;
                servoRotate.write(currentAngle);
                stateStartTime = millis();
                currentState = PAUSE_BEFORE_GRABBING;  
                buttonPressed = false;
            }
            break;
        
        case PAUSE_BEFORE_GRABBING:
            if (millis() - stateStartTime >= 500) {   
                Serial.println("准备开始夹取！");
                stateStartTime = millis();
                currentState = GRABBING;
            }
            break;


        case GRABBING:
             if (millis() - stateStartTime >= 500) {
                Serial.println("夹取中...");
                servoLeft.write(30);
                servoRight.write(30);
                stateStartTime = millis();
                currentState = GRABBING_CHECK;   
            }
            break;

        case GRABBING_CHECK:
            if (millis() - stateStartTime >= 500) {   
                long confirmDistance = readDistanceCM(trig1, echo1);
                if (confirmDistance < 10) {
                    Serial.println("确认：夹取成功！");
                    currentState = ROTATING;
                } else {
                    Serial.println("警告：夹取失败，未检测到物体！");
                    servoLeft.write(90);
                    servoRight.write(90);
                    currentState = SEARCHING;
                }
                stateStartTime = millis();
            }
            break;
            

        case ROTATING:
            if (millis() - stateStartTime >= 500) {
              Serial.println("旋转到投放位置...");
              slowMove(servoRotate, currentAngle, 180, 20);   
              currentAngle = 180;  
              stateStartTime = millis();
              currentState = PAUSE_BEFORE_DROPPING;
           }
           break;

        case PAUSE_BEFORE_DROPPING:
            if (millis() - stateStartTime >= 500) {  
                Serial.println("准备投放...");
                stateStartTime = millis();
                currentState = DROPPING;
            }
            break;


        case DROPPING:
            if (millis() - stateStartTime >= 500) {
                Serial.println("投放完成，张开夹爪");
                servoLeft.write(90);   
                servoRight.write(90);
                stateStartTime = millis();
                currentState = RETURNING;
            }
            break;

        case RETURNING:
            if (millis() - stateStartTime >= 500) {
                Serial.println("回归初始位置，重新开始搜索");
                slowMove(servoRotate,currentAngle,lastFoundAngle, 20);
                currentAngle = lastFoundAngle;
                stateStartTime = millis();
                currentState = SEARCHING;
            }
            break;
    
  
     }

}