// A4988引脚连接Arduino引脚编号
#include <AccelStepper.h>

const int dirPin   = 46;   // Direction
const int stepPin  = 47;   // Step
const int limit_forward   = 28;   // Ms3
const int limit_back   = 30;   // Ms2

const int Pin_GreenLight   = 4;
const int Pin_YellowLight  = 6;
const int Pin_RedLight   = 8;
const int Pin_Buzzer   = 10;

// 步进电机旋转一周步数
const int STEPS_PER_REV = 1000;

char cmd[10];  //用户指令字符
int data;  //用户指令数据
int motorSpeed = 300;  //电机转速(数值越小速度越小)
AccelStepper stepper(4, stepPin, dirPin);
unsigned long startTime;
unsigned long endTime;
unsigned long executionTime;
String Get_Command;

void setup() {
  // 设置引脚模式
  pinMode(stepPin, OUTPUT);
  pinMode(dirPin, OUTPUT);
  pinMode(limit_forward, INPUT);
  pinMode(limit_back, INPUT);

  pinMode(Pin_GreenLight, OUTPUT);
  pinMode(Pin_YellowLight, OUTPUT);
  pinMode(Pin_RedLight, OUTPUT);
  pinMode(Pin_Buzzer, OUTPUT);

  // 初始化电机步进模式为1/16步进
  //digitalWrite(ms1Pin, HIGH);
  //digitalWrite(ms2Pin, HIGH);
  //digitalWrite(ms3Pin, HIGH);



  Serial.begin(9600);
//  Serial.println("Please input motor command:");
  digitalWrite(dirPin, HIGH);
  digitalWrite(limit_forward, HIGH);
  digitalWrite(limit_back, HIGH);

  //  stepper.setMaxSpeed(100000);  // 设置最大速度（根据你的需要调整）
  //  stepper.setSpeed(50);      // 设置起始速度（根据你的需要调整）
}
void loop() {
  if (Serial.available()) {
    // 讀取傳入的字串直到"\n"結尾
    Get_Command = Serial.readStringUntil('\n');
    if (Get_Command == "Move") {
      move();
      Serial.println("OK"); // Respond to the computer
    } 
    else if (Get_Command == "Green") {
      Serial.println("Green Light On");
      digitalWrite(Pin_GreenLight, HIGH);
    } 
    else if (Get_Command == "Yellow") {
      Serial.println("Yellow Light On");
      digitalWrite(Pin_YellowLight, HIGH);
    } 
    else if (Get_Command == "Red") {
      Serial.println("Red Light On");
      digitalWrite(Pin_RedLight, HIGH);
    } 
    else if (Get_Command == "Buzzer") {
      Serial.println("Buzzer On");
      digitalWrite(Pin_Buzzer, HIGH);
    } 
    else if (Get_Command == "Off") {
      Serial.println("All Off");
      digitalWrite(Pin_GreenLight, LOW);
      digitalWrite(Pin_YellowLight, LOW);
      digitalWrite(Pin_RedLight, LOW);
      digitalWrite(Pin_Buzzer, LOW);
    } 
    else {
      Serial.println("你想做什麼?");
    }
  }
}
void move()
{
  if (digitalRead(limit_back) == LOW)
  {
    digitalWrite(dirPin, LOW);
    while (true)
    {
      if (digitalRead(limit_forward) == LOW) {
        Serial.println("Motor emergency stop");
        break;
      }
      digitalWrite(stepPin, HIGH);
      delayMicroseconds(motorSpeed);
      digitalWrite(stepPin, LOW);
      delayMicroseconds(motorSpeed);
    }

  }
  else
  {
    digitalWrite(dirPin, HIGH);
    while (true)
    {
      if (digitalRead(limit_back) == LOW) {
        Serial.println("Motor emergency stop");
        break;
      }
      digitalWrite(stepPin, HIGH);
      delayMicroseconds(motorSpeed);
      digitalWrite(stepPin, LOW);
      delayMicroseconds(motorSpeed);
    }
  }
}