#include <Wire.h>
#include <MPU6050.h>
#include "HX711.h"

// 传感器引脚定义
const int FSR_PIN = 34;
const int TRIG_PIN = 26;
const int ECHO_PIN = 27;
const int LOADCELL_DOUT_PIN = 32;
const int LOADCELL_SCK_PIN = 33;
const int BUTTON_PIN = 18;  // 单按钮引脚

// 传感器对象
MPU6050 mpu;
HX711 scale;

// 全局变量
float calibration_factor = 2280;
bool systemRunning = true;  // true=读取模式  false=急停模式

void setup() {
  Serial.begin(115200);

  // 按钮初始化（内部上拉）
  pinMode(BUTTON_PIN, INPUT_PULLUP);

  // 初始化 MPU6050
  Wire.begin();
  mpu.initialize();
  if (!mpu.testConnection()) {
    Serial.println("MPU6050 失败！");
    while (1);
  }
  mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_2000);
  mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_2);

  // 初始化其他传感器
  pinMode(FSR_PIN, INPUT);
  scale.begin(LOADCELL_DOUT_PIN, LOADCELL_SCK_PIN);
  scale.set_scale(calibration_factor);
  scale.tare();
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);

  Serial.println("\n===== 初始化完成 =====");
  Serial.println("按钮：切换 读取模式 / 急停模式");
  Serial.println("========================\n");
}

void loop() {
  // 极简按钮检测：按下就切换状态（无防抖，适合仿真）
  if (digitalRead(BUTTON_PIN) == LOW) {
    systemRunning = !systemRunning;
    delay(200);  // 只留一点点延时防止仿真连闪
  }

  if (systemRunning) {
    // 正常读取模式
    readAllSensors();
    delay(300);
  } else {
    // 急停模式
    Serial.println("【急停模式】停止读取");
    delay(300);
  }
}

void readAllSensors() {
  readMPU6050();
  readFSR402();
  readHX711();
  readHC_SR04();
  Serial.println("----------");
}

void readMPU6050() {
  int16_t ax, ay, az, gx, gy, gz;
  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

  float accelX = ax / 16384.0;
  float accelY = ay / 16384.0;
  float accelZ = az / 16384.0;

  float gyroX = gx / 16.4;
  float gyroY = gy / 16.4;
  float gyroZ = gz / 16.4;

  Serial.printf("加速度 X:%.2fg Y:%.2fg Z:%.2fg | ", accelX, accelY, accelZ);
  Serial.printf("陀螺仪 X:%.2f°/s Y:%.2f°/s Z:%.2f°/s\n", gyroX, gyroY, gyroZ);
}

void readFSR402() {
  int rawValue = analogRead(FSR_PIN);
  float voltage = rawValue * (3.3 / 4095.0);

  Serial.printf("FSR ADC值: %d | 电压: %.2fV | 压力: ", rawValue, voltage);
  if (rawValue < 100)      Serial.println("无压力");
  else if (rawValue < 500) Serial.println("轻压");
  else if (rawValue < 800) Serial.println("中压");
  else                     Serial.println("重压");
}

void readHX711() {
  float weight = scale.get_units(5);
  Serial.printf("重量: %.2f kg\n", weight);
}

void readHC_SR04() {
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);

  long duration = pulseIn(ECHO_PIN, HIGH);
  long distance = duration * 0.034 / 2;

  Serial.printf("距离: %ld cm", distance);
  if (distance < 10)      Serial.println(" - 很近!");
  else if (distance < 50) Serial.println(" - 近距离");
  else if (distance < 200) Serial.println(" - 中距离");
  else                    Serial.println(" - 远距离");
}