//StateMachine 2024 oda
//240930 製作開始 1008一先ず動く
//RUN押し、wait後に前進して前進端センサで停止、wait後に後退、

//ライブラリ
#include <LiquidCrystal_I2C.h>
LiquidCrystal_I2C lcd(0x27, 16, 2);

//各種設定

//動作回数設定
#define count 100  //この回数だけ動く

unsigned long countSettingValue = count; //目標回数
unsigned long countProcessValue = 97; //実行回数


//計時（動作用）
unsigned long currentMillis = 0;
unsigned long previousMillis = 0;
unsigned long _currentMillis = 0;//一時記憶
unsigned long _previousMillis = 0;


unsigned long stopMillis = 0;

unsigned long lcdCurrentMillis = 0;
unsigned long lcdPreviousMillis = 0;


//時間設定
#define t1 3  //送り前wait 1s
#define t2 10  //送り時限停止 30s
#define t3 3  //戻り前wait 1s
#define t4 10  //戻り時限停止 30s

int time1 = t1 * 1000;
int time2 = t2 * 1000;
int time3 = t3 * 1000;
int time4 = t4 * 1000;

/*
  //計時（表示用）
  unsigned long currentTime = 0;
  unsigned long previousTime = 0;
  unsigned long progressTime = 0;


  //時計
  unsigned long previousTime= 0;//time_countに0を代入する
  unsigned long second = 0;
  unsigned long minute = 0;
  unsigned long hour = 0;


  void progressTimeCount(){
  previousTime = millis();
  second = (millis() - previousMillis) / 1000;
  minute = second / 60;
  hour = minute / 60;

  lcd.setCursor(8,1);
  lcd.print(hour);
  lcd.setCursor(10,1);
  lcd.print(":");
  lcd.setCursor(11,1);
  lcd.print(minute%60);
  lcd.setCursor(13,1);
  lcd.print(":");
  lcd.setCursor(14,1);
  lcd.print(second%60);
  }
*/

//機器の状態
byte machineState = 0;
byte _machineState = 0; //一時記憶用

//Input Pin
const byte runButton = 11;  //動作開始ボタン
const byte stopButton = 10;  //停止ボタン
const byte limit1 = 9;  //リミットスイッチ　開始端
const byte limit2 = 8;  //リミットスイッチ　終端

//Output Pin
const byte runLed = 7; //動作中の表示
const byte stopLed = 6; //停止中の表示
const byte output1 = 5;  //モータON用接点　CW　開始端から終端方向
const byte output2 = 4;  //モータON用接点　CCW　終端から開始端方向
const byte alertLed = 3;




//ButtonRead & state
boolean currentRunButton = HIGH;
boolean previousRunButton = HIGH;
boolean runState = false;
boolean runLatch = false;

boolean currentStopButton = HIGH;
boolean previousStopButton = HIGH;
boolean stopState = false;

boolean currentLimit1 = HIGH;
boolean previousLimit1 = LOW;
boolean limitState1 = false;

boolean currentLimit2 = HIGH;
boolean previousLimit2 = LOW;
boolean limitState2 = false;

boolean outputState1 = false; //出力中?
boolean outputState2 = false; //出力中?

unsigned long rap;  //動作回数



//セットアップ
void setup() {
  //各種ピン設定
  Serial.begin(9600);
  //  Serial.begin(115200);
  pinMode(runButton, INPUT_PULLUP);
  pinMode(stopButton, INPUT_PULLUP);
  pinMode(limit1, INPUT_PULLUP);
  pinMode(limit2, INPUT_PULLUP);
  pinMode(output1, OUTPUT);
  pinMode(output2, OUTPUT);
  pinMode(runLed, OUTPUT);
  pinMode(stopLed, OUTPUT);

  lcd.init();
  lcd.backlight();
  lcd.clear();

  lcd.setCursor(0, 0);
  lcd.print("*STATE MACHINE* ");
  lcd.setCursor(0, 1);
  lcd.print("FLOWELL  2024/10");

  delay(3000);
  lcd.clear();
  currentMillis = millis();
  previousMillis = currentMillis;
}






//メイン
void loop() {

  //テスト用
  /*
    Serial.print(" cRUN:");
    Serial.print(currentRunButton);
    Serial.print(" pRUN:");
    Serial.print(previousRunButton);

    Serial.print(" RUNs:");
    Serial.print(runState);
    Serial.print(" RUNl:");
    Serial.print(runLatch);

    Serial.print(" Mst:");
    Serial.print(machineState);
    Serial.print(" _Mst:");
    Serial.print(_machineState);


    Serial.print(" cLS1:");
    Serial.print(currentLimit1);
    Serial.print(" pLS1:");
    Serial.print(previousLimit1);
    Serial.print(" sLS1:");
    Serial.print(limitState1);

    Serial.print(" cLS2:");
    Serial.print(currentLimit2);
    Serial.print(" pLS2:");
    Serial.print(previousLimit2);
    Serial.print(" sLS2:");
    Serial.print(limitState2);

    Serial.print(" ops1:");
    Serial.print(outputState1);

    Serial.print(" cutPV:");
    Serial.print(countProcessValue);





    Serial.print(" cst:");
    Serial.print(currentStopButton);
    Serial.print(" pst:");
    Serial.print(previousStopButton);
    Serial.print(" sst:");
    Serial.print(stopState);

    Serial.print(" Cmil:");
    Serial.print(currentMillis);
    Serial.print(" Pmil:");
    Serial.print(previousMillis);




    Serial.println("");
  */

  //count表示
  if (countProcessValue < 10) {
    lcd.setCursor(15, 0);
    lcd.print(countProcessValue);
  } else if (countProcessValue >= 10 && countProcessValue < 100) {
    lcd.setCursor(14, 0);
    lcd.print(countProcessValue);
  } else if (countProcessValue >= 100 && countProcessValue < 1000) {
    lcd.setCursor(13, 0);
    lcd.print(countProcessValue);
  } else if (countProcessValue >= 1000 && countProcessValue < 10000) {
    lcd.setCursor(12, 0);
    lcd.print(countProcessValue);
  } else if (countProcessValue >= 10000 && countProcessValue < 100000) {
    lcd.setCursor(11, 0);
    lcd.print(countProcessValue);
  } else if (countProcessValue >= 100000 && countProcessValue < 1000000) {
    lcd.setCursor(10, 0);
    lcd.print(countProcessValue);
  }

  //count設定回数到達でstate5
  if (countSettingValue == countProcessValue) {
    machineState = 5;
  }

  //ReadButon
  currentRunButton = digitalRead(runButton);
  currentStopButton = digitalRead(stopButton);
  currentLimit1 = digitalRead(limit1);
  currentLimit2 = digitalRead(limit2);

  //PressRunButton
  if (currentRunButton == LOW && previousRunButton == HIGH) {
    runState = true;
    runLatch = true;
  }
  previousRunButton = currentRunButton;


  //Press STOP Button STOP押したらstate6（run以外無反応）
  if (runState == true){
    if(outputState1 == true || outputState2 == true){
    if (currentStopButton == LOW && previousStopButton == HIGH) {
      stopMillis = millis();
      runState = false;
      stopState = true;

      //  _currentMillis = currentMillis;
      //  _previousMillis = previousMillis;
      _machineState = machineState;
      machineState = 6;
    }
    previousStopButton = currentStopButton;
  }
  }


  //machineState 0 to 1
  if (runLatch == true && machineState == 0) {
    machineState = 1;
    runLatch = false;
    lcd.setCursor(0, 0);
    lcd.print("                 ");
    lcd.setCursor(0, 1);
    lcd.print("                 ");
  }


  //machineState

  switch (machineState) {

    case 0:

      currentMillis = millis();
      previousMillis = currentMillis;

      lcd.setCursor(0, 0);
      lcd.print("READY ");

      //文字点滅
      lcdCurrentMillis = millis();

      if (lcdCurrentMillis - lcdPreviousMillis  < 500) {
        lcd.setCursor(0, 1);
        lcd.print("Press Run START!");
      }
      else if (lcdCurrentMillis - lcdPreviousMillis > 500) {
        lcd.setCursor(0, 1);
        lcd.print("                ");
      }
      if (lcdCurrentMillis - lcdPreviousMillis > 1000) {
        lcdPreviousMillis = lcdCurrentMillis;
      }



      break;

    case 1: //送りwait
      lcd.setCursor(0, 0);
      lcd.print("WAIT F");//6文字
      lcd.setCursor(0, 1);
      lcd.print("Forward  *WAIT* ");

      digitalWrite(runLed, HIGH);

      currentMillis = millis();
      if (currentMillis - previousMillis >= time1) {
        machineState = 2;
        previousMillis = currentMillis;
      }

      break;





    case 2: //送り


      digitalWrite(runLed, HIGH);
      digitalWrite(output1, outputState1);

      currentMillis = millis();

      if (currentLimit2 == LOW && previousLimit2 == HIGH) {
        limitState2 = true;
        //  countProcessValue++;
        previousLimit2 = currentLimit2;
      } else if (currentLimit2 == HIGH && previousLimit2 == LOW) {
        limitState2 = false;
        previousLimit2 = currentLimit2;
      }

      if (limitState2 == true) {
        digitalWrite(output1, LOW);
        digitalWrite(output2, LOW);
        machineState = 3;
        previousMillis = currentMillis;

      }

      if (limitState2 == false) { //押されていない

//move F
        if (currentMillis - previousMillis <= time2) {
          outputState1 = true;
          digitalWrite(output1, HIGH);
          digitalWrite(output2, LOW);
          lcd.setCursor(0, 0);
          lcd.print("MOVE F");
          lcd.setCursor(0, 1);
          lcd.print("Forward  *MOVE* ");

//error F
        } else if (currentMillis - previousMillis >= time2) {
          outputState1 = false;
          digitalWrite(output1, LOW);
          digitalWrite(output2, LOW);
          digitalWrite(alertLed, HIGH);
          lcd.setCursor(0, 0);
          lcd.print("ERROR*F");
          lcd.setCursor(0, 1);
          lcd.print("Forward *ERROR* ");
        }
      }

      break;





    case 3: //wait
      currentMillis = millis();
      lcd.setCursor(0, 0);
      lcd.print("WAIT B");
      lcd.setCursor(0, 1);
      lcd.print("Backward *WAIT* ");

      digitalWrite(output1, LOW); //モータ出力CW　青LED7 オフ
      digitalWrite(output2, LOW); //モータ出力CCW　黄LED6 オフ

      if (currentMillis - previousMillis >= time3) {
        machineState = 4;
        previousMillis = currentMillis;

      }
      break;



    case 4: //戻り


      digitalWrite(output2, outputState2);

      currentMillis = millis();

      if (currentLimit1 == LOW && previousLimit1 == HIGH) {
        limitState1 = true;
        countProcessValue++;
        previousLimit1 = currentLimit1;
      } else if (currentLimit1 == HIGH && previousLimit1 == LOW) {
        limitState1 = false;
        previousLimit1 = currentLimit1;
      }


      if (limitState1 == true) {
        digitalWrite(output1, LOW);
        digitalWrite(output2, LOW);
        machineState = 1;
        previousMillis = currentMillis;
      }

//MOVE B
      if (limitState1 == false) { //押されていない

        if (currentMillis - previousMillis <= time4) {
          outputState2 = true;
          digitalWrite(output1, LOW);
          digitalWrite(output2, HIGH);
          lcd.setCursor(0, 0);
          lcd.print("MOVE B");
          lcd.setCursor(0, 1);
          lcd.print("Backward *MOVE* ");

//ERROR B

        } else if (currentMillis - previousMillis >= time4) {
          outputState2 = false;
          digitalWrite(output1, LOW);
          digitalWrite(output2, LOW);
          digitalWrite(alertLed, HIGH);
          lcd.setCursor(0, 0);
          lcd.print("ERROR*B");
          lcd.setCursor(0, 1);
          lcd.print("Backward *ERROR*");
        }
      }

      break;

    case 5: //目標回数到達時の動作
      runState = false;
      digitalWrite(runLed, runState);
      digitalWrite(alertLed, HIGH);

      digitalWrite(output1, LOW);
      digitalWrite(output2, LOW);

      lcd.setCursor(0, 0);
      lcd.print("*DONE*");

      //文字点滅
      lcdCurrentMillis = millis();

      if (lcdCurrentMillis - lcdPreviousMillis  < 500) {
        lcd.setCursor(0, 1);
        lcd.print("TargetValue*END*");
        digitalWrite(alertLed, HIGH);
      }
      else if (lcdCurrentMillis - lcdPreviousMillis > 500) {
        lcd.setCursor(0, 1);
        lcd.print("                ");
        digitalWrite(alertLed, LOW);
      }
      if (lcdCurrentMillis - lcdPreviousMillis > 1000) {
        lcdPreviousMillis = lcdCurrentMillis;
      }







      break;

    case 6: //stop

      previousMillis =  currentMillis = millis();

      digitalWrite(runLed, LOW);
      digitalWrite(alertLed, HIGH);

      digitalWrite(output1, LOW);
      digitalWrite(output2, LOW);


      lcd.setCursor(0, 1);
      lcd.print("STOP to Continue");


      //文字点滅
      lcdCurrentMillis = millis();

      if (lcdCurrentMillis - lcdPreviousMillis  < 250) {
        lcd.setCursor(0, 0);
        lcd.print("*PAUSE");
        digitalWrite(stopLed, HIGH);
      }
      else if (lcdCurrentMillis - lcdPreviousMillis > 250) {
        lcd.setCursor(0, 0);
        lcd.print("------");
        digitalWrite(stopLed, LOW);
      }
      if (lcdCurrentMillis - lcdPreviousMillis > 500) {
        lcdPreviousMillis = lcdCurrentMillis;
      }





      currentStopButton = digitalRead(stopButton);

      if (currentStopButton == LOW && previousStopButton == HIGH) {



        stopState = false;
        runState = true;

        digitalWrite(runLed, HIGH);
        digitalWrite(stopLed, LOW);
        digitalWrite(alertLed, LOW);

        machineState = _machineState;

      }

      previousStopButton = currentStopButton;

      break;

  }

}