// 多电机精确时间控制版本

// 电机物理参数（固定值）
const float STEP_ANGLE = 1.8;    // 步距角（度）
const int MICROSTEPS = 1;        // 细分数
const float MM_PER_REVOLUTION = 2.0;  // 每圈对应丝杠移动距离（mm）
const int STEPS_PER_REVOLUTION = 360 / (STEP_ANGLE / MICROSTEPS);  // 每圈步数(200)

// 定义电机参数结构体
typedef struct {
  int dirPin;        // 方向引脚
  int stepPin;       // 步进引脚
  int targetSteps;   // 目标步数
  int currentSteps;  // 当前步数
  unsigned long stepDelay; // 步进间隔（微秒）
  bool direction;    // 方向（HIGH/LOW）
  bool isRunning;    // 运行状态
  unsigned long lastStepTime; // 上次步进时间
  unsigned long startTime;   // 启动时间
  unsigned long totalTime;   // 总时间（毫秒）
} StepperMotor;

// 初始化电机参数（适配Arduino Uno引脚）
StepperMotor motors[] = {
  {2, 3, 0, 0, 0, HIGH, false, 0, 0, 0},  // 电机1: DIR=2, STEP=3
  {4, 5, 0, 0, 0, LOW,  false, 0, 0, 0},  // 电机2: DIR=4, STEP=5
  {6, 7, 0, 0, 0, HIGH, false, 0, 0, 0}   // 电机3: DIR=6, STEP=7
};

const int motorCount = sizeof(motors)/sizeof(StepperMotor); // 电机数量

// 计算并设置电机参数
void setMotorMovement(int motorIndex, float distance, unsigned long timeMs, bool dir) {
  if (motorIndex < 0 || motorIndex >= motorCount) return;
  
  // 计算目标步数
  float revolutions = distance / MM_PER_REVOLUTION;
  int steps = (int)(revolutions * STEPS_PER_REVOLUTION);
  
  // 计算所需步进间隔（微秒）
  // 总时间 = 步数 × 每步时间(stepDelay/1000)
  unsigned long delayUs = (timeMs * 1000) / max(1, steps);
  
  // 设置电机参数
  motors[motorIndex].targetSteps = steps;
  motors[motorIndex].stepDelay = delayUs;
  motors[motorIndex].direction = dir;
  motors[motorIndex].totalTime = timeMs;
  
  Serial.print("Motor ");
  Serial.print(motorIndex+1);
  Serial.print(" configured: ");
  Serial.print(distance);
  Serial.print("mm in ");
  Serial.print(timeMs);
  Serial.println("ms");
}

// 启动单个电机
void startMotor(int motorIndex) {
  if (motorIndex < 0 || motorIndex >= motorCount) return;
  
  motors[motorIndex].currentSteps = 0;
  motors[motorIndex].isRunning = true;
  motors[motorIndex].startTime = millis();
  motors[motorIndex].lastStepTime = micros();
  
  Serial.print("Motor ");
  Serial.print(motorIndex+1);
  Serial.println(" started!");
}

// 同时启动多个电机
void startMotorsTogether(int* motorIndices, int count) {
  unsigned long currentTime = millis();
  
  for (int i = 0; i < count; i++) {
    int index = motorIndices[i];
    if (index >= 0 && index < motorCount) {
      motors[index].currentSteps = 0;
      motors[index].isRunning = true;
      motors[index].startTime = currentTime;
      motors[index].lastStepTime = micros();
      
      Serial.print("Motor ");
      Serial.print(index+1);
      Serial.println(" started!");
    }
  }
}

// 处理所有电机步进
void handleMotors() {
  unsigned long currentTime = micros();
  
  for (int i = 0; i < motorCount; i++) {
    StepperMotor &m = motors[i];
    
    if (m.isRunning && (currentTime - m.lastStepTime >= m.stepDelay)) {
      // 发送步进脉冲
      digitalWrite(m.stepPin, HIGH);
      digitalWrite(m.dirPin, m.direction);
      delayMicroseconds(5); // 确保脉冲宽度
      digitalWrite(m.stepPin, LOW);
      
      m.currentSteps++;
      m.lastStepTime = currentTime;
      
      // 检查是否完成目标步数
      if (m.currentSteps >= m.targetSteps) {
        m.isRunning = false;
        unsigned long elapsedTime = millis() - m.startTime;
        
        Serial.print("Motor ");
        Serial.print(i+1);
        Serial.print(" completed ");
        Serial.print(m.targetSteps);
        Serial.print(" steps in ");
        Serial.print(elapsedTime);
        Serial.println("ms");
      }
    }
  }
}

// 检查所有电机是否完成运行
bool allMotorsStopped() {
  for (int i = 0; i < motorCount; i++) {
    if (motors[i].isRunning) return false;
  }
  return true;
}

void setup() {
  Serial.begin(115200);
  
  // 初始化引脚
  for (int i = 0; i < motorCount; i++) {
    pinMode(motors[i].dirPin, OUTPUT);
    pinMode(motors[i].stepPin, OUTPUT);
    digitalWrite(motors[i].stepPin, LOW); // 初始低电平
  }
  
  Serial.println("System initialized!");
  Serial.print("Steps per revolution: ");
  Serial.println(STEPS_PER_REVOLUTION);
}

void loop() {
  // 示例：5000ms内协同控制多电机
  if (!allMotorsStopped()) {
    handleMotors();  // 持续处理电机步进
  } else {
    // 所有电机完成后，等待2秒再启动下一组动作
    delay(2000);
    
    // 设置电机1: 正向移动2mm，5000ms完成
    setMotorMovement(0, 2.0, 5000, HIGH);
    
    // 设置电机2: 反向移动4mm，5000ms完成
    setMotorMovement(1, 4.0, 5000, LOW);
    
    // 设置电机3: 正向移动4mm，5000ms完成
    setMotorMovement(2, 4.0, 5000, HIGH);
    
    // 同时启动所有电机
    int motorIndices[] = {0, 1, 2};
    startMotorsTogether(motorIndices, 3);
  }
  
  // 处理其他任务（如串口指令）
  if (Serial.available()) {
    String command = Serial.readStringUntil('\n');
    // 此处可添加串口指令解析代码
  }
  
  delay(1); // 减少CPU占用
}