/*
 녹색 버튼 1회 = 0도 90도 180도 자동 운행                                  
 노란 버튼 1회 = 키 입력 모드 0 ~ 180 입력
 빨간 버튼 1회 = 정지
*/

#include <Servo.h> // 서보 모터 라이브러리
#include <Keypad.h>

// 라이브러리 객체 생성
Servo servo;

// 버튼 생성
const int start_btn = 8;
const int semiauto_btn = 9;
const int stop_btn = 10;

// 눌린 버튼 저장할 값 
int automatic = 0;
bool semiautoMessageSent = false; // 세미 오토 메시지를 출력


// 키패드 설정 2차원 배열 만들기
const uint8_t ROWS = 4;
const uint8_t COLS = 4;
char keys[ROWS][COLS] = {
  {'1', '2', '3', 'A'},
  {'4', '5', '6', 'A'},
  {'7', '8', '9', 'A'},
  {'*', '0', '#', 'A'}
};

uint8_t ROWSPIN[ROWS] = {13, 12, 7, 6};
uint8_t COLSPIN[COLS] = {5, 4, 3, 2};
Keypad keypad(makeKeymap(keys), ROWSPIN, COLSPIN, ROWS, COLS);



void setup() {
  servo.attach(11); // 서보 모터 초기화
  pinMode(start_btn, INPUT); // 자동 버튼 초기화
  pinMode(semiauto_btn, INPUT); // 수동 버튼 초기화
  pinMode(stop_btn, INPUT); // 정지 버튼
  Serial.begin(9600);
}

void loop() {
  int start = digitalRead(start_btn); // 자동 반복
  int manual = digitalRead(semiauto_btn); // 수동 조작
  int stop = digitalRead(stop_btn); // 정지
  
  if (start == HIGH) {
    automatic = 1;
  } else if (manual == HIGH) {
    automatic = 2;
  } else if (stop == HIGH) {
    automatic = 0;
    semiautoMessageSent = false;
  }

  if (automatic == 1) {
    autoServo(400);
  } else if (automatic ==2) {
    semiauto();
  }
}

void autoServo(int time) {
  
  for (int i = 0; i <= 180; i += 90) {
    servo.write(i);
    if(digitalRead(stop_btn)==1){automatic = 0;return;}
    delay1(time);
    if(automatic==0){return;}
  }
  delayMicroseconds(20);
  for (int i = 180; i >= 0; i -= 90) {
    servo.write(i);
    if(digitalRead(stop_btn)==1){automatic = 0;return;}
    delay1(time);
    if(automatic==0){return;}
  }
}

void semiauto() {
  if (!semiautoMessageSent) { // 세미 오토 메시지를 아직 출력하지 않은 경우에만 실행
    Serial.println("Servo Moter move to value"); // 메시지 출력
    semiautoMessageSent = true; // 메시지를 출력했음을 표시
  }

  if (Serial.available() > 0) {
    int input = Serial.parseInt(); // 정수로 입력값을 읽음
    

    if (input >= 0 && input <= 180) { // 유효한 각도 범위 확인
      servo.write(input); // 서보 모터 각도 설정
  
      Serial.read();
      delay(100);
    }
  }
}
void delay1(int time){
  int tt=time/50;
  for(int i=0;i<tt;i++){
    if(digitalRead(stop_btn)==1){automatic = 0;return;}
    delay(50);
  }
}