#ifndef INC_SERVO_H_
#define INC_SERVO_H_

#include "main.h"
#include "delay_us.h"

// Servo 연결 Timer Channel CCR
#define SERVO_CCR		TIM3->CCR1

extern int8_t servoDirec;
extern int8_t servoPos;
extern int8_t servoEnabled;

void servoInit();
void servoMove(uint8_t mode, uint8_t pos);

#endif /* INC_SERVO_H_ */

// servo.c
#include "servo.h"

int8_t servoDirec;
int8_t servoPos;
int8_t servoEnabled;

void servoInit() {
	servoDirec = 0;
	servoPos = 30;
	servoEnabled = 1;
	SERVO_CCR = servoPos;
}

// Servo 동작 함수, mode 0 범위내 위치 이동 / 1 왕복 / 2 1번 왕복 / 3 제한 없이 위치 이동
void servoMove(uint8_t mode, uint8_t pos) {
	if (servoEnabled == 1) {
		switch (mode) {
			case 0:
				if (pos <= 30) {
					pos = 30;
				}
				else if(pos >= 120) {
					pos = 120;
				}

				if (servoPos < pos) {
					servoPos++;
				}
				else if (servoPos > pos) {
					servoPos--;
				}
				break;

			case 1:
				if (servoPos <= 30) {
					servoDirec = 1;
				}
				else if (servoPos >= 120) {
					servoDirec = 0;
				}

				if (servoDirec == 0) {
					servoPos--;
				}
				else if (servoDirec == 1) {
					servoPos++;
				}
				break;

			case 2:
				if (servoPos <= 120) {
					servoPos++;
				}
				else if (servoPos >= 30) {
					servoPos--;
					if (servoPos == 30) {
						servoEnabled = 0;
					}
				}
				break;

			case 3:
				if (servoPos < pos) {
					servoPos++;
				}
				else if (servoPos > pos) {
					servoPos--;
				}
				break;
		}
	}
	else if (servoEnabled == 0) {	// Servo 동작 정지 상태 Code 위치

	}
	SERVO_CCR = servoPos;			// Servo 위치 조정
}