#include <SPI.h>
#include <RF24.h>
RF24 radio(4, 5);

long long address = 0x1234ABCDEFLL;

struct Value{
  uint16_t value_x;   // servo motor pwm
  uint16_t value_y;   // DC motor pwm
};

Value data;

int angle_x = 90;     // servo initial position (0~180)
int velo_y = 0;       // DC motor inital velocity
int direction_y = 0;  // DC motor direction
int i = 0;            // joystick center position calculation counter
int center_x = 0;     // joystick initial center_x 
int center_y = 0;     // joystick initial center_y
int ref_xl, ref_xr, ref_ya, ref_yb;    // Servo & DC rotation reference value from center position value

//servo setting
const int PIN_SERVO = 15;             // servo pwm pin
const int CH_SERVO = 2;               // servo pwm channel
const int servoFrequency = 50;        // servo pwm frequency (Hz)
const int servoResolution = 16;       // servo pwm resolution (bit) (12bit: 0~4095)

// RecvTime
unsigned long lastRecvTime = 0;

void centerData() {
  while(i < 100) {
    if(radio.available()){
      radio.read(&data, sizeof(Value));
      center_x = center_x + data.value_x;
      center_y = center_y + data.value_y;
      i++;
    }
  }
  center_x = center_x / 100;         // x축 조이스틱 센터값 (평균)
  center_y = center_y / 100;         // y축 조이스틱 센터값 (평균)

  ref_xl = center_x / 2;             // 좌회전 동작 기준값
  ref_xr = (4095 + center_x) / 2;    // 우회전 동작 기준값
  ref_ya = (4095 + center_y) / 2;    // accelator 동작 기준값
  ref_yb = center_y / 2;             // brake 동작 기준값
}

void recvData(){
  while(radio.available()){
    radio.read(&data, sizeof(Value));
    lastRecvTime = millis();  
  }
}

/*
// TB6612FNG right side & ESP32-WROOM-32D DEVKIT_C V4 left side pin order
const int PIN_PWMA = 32;  
const int PIN_INA1 = 33;
const int PIN_INA2 = 25;
const int PIN_STBY = 26;
const int PIN_INB1 = 27;
const int PIN_INB2 = 14;
const int PIN_PWMB = 12;
*/

// TB6612FNG right side & ESP32-WROOM-32 left side pin order
const int PIN_PWMA = 33;
const int PIN_AIN2 = 25;
const int PIN_AIN1 = 26;
const int PIN_STBY = 27;
const int PIN_BIN1 = 14;
const int PIN_BIN2 = 12;
const int PIN_PWMB = 13;

// channel setting (GPIO pin, channel)
// if STBY(CH0) = LOW, Standby states.
// if STBY(CH0) = HIGH, and INA1&INA2 have different value
// and PWMA has some value, CW or CCW rotation is made.

// Channel      CW   CCW  Stanby Stop Brake1 Brake2 Brake3
// PIN_STBY (0) HIGH HIGH LOW    HIGH  HIGH  HIGH   HIGH
// PIN_INA1 (1) HIGH LOW  *      LOW   HIGH  LOW    HIGH
// PIN_INA2 (2) LOW  HIGH *      LOW   HIGH  HIGH   LOW
// CH_PWMA  (3) PWM  PWM  *      HIGH  *     LOW    LOW

// PWM Channel setup
const int CH_PWMA = 0;
const int CH_PWMB = 1;
// PWM frequency and bit resolution setup
const int pwmFrequency = 10000;  // Hz
const int bitResolution = 8;     // pwm value: 0~255

// led setup
const int PIN_LED = 2;
int ledstate;


void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);

  radio.begin();
  radio.openReadingPipe(1, address);
  radio.setPALevel(RF24_PA_LOW);
  radio.startListening();

  // Servo PWM Setup
  ledcSetup(CH_SERVO, servoFrequency, servoResolution);  // PWM CH2, Frequncy 50 Hz, 12bit resolution
  ledcAttachPin(PIN_SERVO, CH_SERVO);                    // PWM CH2을 GPIO 15번으로 출력

  // center value
  centerData();
  
  pinMode(PIN_LED, OUTPUT);

  // tb6612fng setting
  pinMode(PIN_STBY, OUTPUT);
  pinMode(PIN_AIN1, OUTPUT);
  pinMode(PIN_AIN2, OUTPUT);
  pinMode(PIN_PWMA, OUTPUT);
  pinMode(PIN_BIN1, OUTPUT);
  pinMode(PIN_BIN2, OUTPUT);
  pinMode(PIN_PWMB, OUTPUT);
  
  // motor output(PWM) setting (channel, frequency, bit)
  ledcSetup(CH_PWMA, pwmFrequency, bitResolution);
  ledcSetup(CH_PWMB, pwmFrequency, bitResolution);
  ledcAttachPin(PIN_PWMA, CH_PWMA);
  ledcAttachPin(PIN_PWMB, CH_PWMB);
}

void loop() {
  ledstate = 0;
  unsigned long now = millis();

  if( now - lastRecvTime > 5){    // 5ms 마다 서보출력
    recvData();
    rotate_xy();

    move(1, velo_y, direction_y);               // motor A(right wheels), velo_y speed, moving direction
    move(2, velo_y, direction_y);               // motor B(left wheels), velo_y speed, moving direction

    if(ledstate > 0) {
      digitalWrite(PIN_LED, HIGH);
    }
    else {
      digitalWrite(PIN_LED, LOW);
    }
  }
}


void move(int motor, int speed, int direction) {   //Move specific motor at speed and direction
//motor: A(Right) -> 1, B(Left) -> 2
//speed: 0 is off, and 255 is full speed
//direction: 0 clockwise, 1 counter-clockwise

  digitalWrite(PIN_STBY, HIGH);         // move

  boolean inPin1 = HIGH;                // Defalut(direction=0) - Clockwise
  boolean inPin2 = LOW;

  if(direction == 1) {                  // Count-clockwise
    inPin1 = LOW;
    inPin2 = HIGH;
  }

  if(motor == 1){                       // if motor == 1, right wheel
    digitalWrite(PIN_AIN1, inPin1);
    digitalWrite(PIN_AIN2, inPin2);
    ledcWrite(CH_PWMA, speed);
  }
  else {                                // if motor is not 1, left wheel
    digitalWrite(PIN_BIN1, inPin1);
    digitalWrite(PIN_BIN2, inPin2);
    ledcWrite(CH_PWMB, speed);
  }
}

void stop() {
  digitalWrite(PIN_STBY, LOW);         // stand-by = stop
}

void rotate_xy() {
  Serial.print("x: "); Serial.print(data.value_x);
  Serial.print(", y: "); Serial.println(data.value_y);

  //x축 서보 구동 조건 설정
  if(data.value_x < ref_xl) {     
    if(angle_x > 55) {                 // servo 55~125 degree
      angle_x--;
    }
    else {
      angle_x = 55;
    }
    ledstate++;
  }
  else if(data.value_x > ref_xr) {
    if(angle_x < 125) {
      angle_x++;
    }
    else {
      angle_x = 125;
    }
    ledstate++;
  }
  else {
    if(angle_x < 90) {
      angle_x++;
    }
    else if(angle_x > 90) {
      angle_x--;
    }
    else {
      angle_x = 90;
    }
  }
  //servo_x.write(angle_x);
  servoWrite(CH_SERVO, angle_x);
  delay(5);                    // delay를 더 줄이면 서보 모터 기어에 무리가 감

  //y축 DC 구동 조건 설정
  if(data.value_y > ref_ya) {
    if(direction_y == 0) {
      if(velo_y < 255) {
        velo_y++;
      }
      else {
        velo_y = 255;
      }
    }
    if(direction_y == 1) {
      if(velo_y > 0) {
        velo_y--;
      }
      else {
        direction_y = 0;
        velo_y = 0;
      }
    }
    ledstate++;
  }
  else if(data.value_y < ref_yb) {
    if(direction_y == 1) {
      if(velo_y < 255) {
        velo_y++;
      }
      else {
        velo_y = 255;
      }
    }
    if(direction_y == 0) {
      if(velo_y > 0) {
        velo_y--;
      }
      else {
        direction_y = 1;
        velo_y = 0;
      }
    }
    ledstate++;
  }
  else {
    if(velo_y > 0) {
      velo_y--;
    }
    else {
      velo_y = 0;
    }
  }
}

// deg는 0~180도 까지
void servoWrite(int ch, int deg)
{
    int servoPWM = map(deg, 0, 180, 2720, 8191);  // 16bit (PWM 4.15% ~ 12.5%)
    ledcWrite(ch, servoPWM);
}

#include <SPI.h>
#include <RF24.h>
RF24 radio(4, 5);

long long address = 0x1234ABCDEFLL;

// for no interference, two joysticks must be used with each other ADC!  
// one joystick use ADC1: GPIO 36,39,32,33,34,35
// the other must use ADC2: GPIO 4,0,2,15,13,12,14,27,25,26
const int Jstick_x_pin = 25;           // Left_Right GPIO25
const int Jstick_y_pin = 32;           // Forward_Back GPIO32

struct Value{
  uint16_t value_x;
  uint16_t value_y;
};

Value data;


void setup() {
  Serial.begin(115200);
  
  radio.begin();
  radio.openWritingPipe(address);
  radio.setPALevel(RF24_PA_MIN);
  radio.stopListening();
}

void loop() {
  data.value_x = Jstick(Jstick_x_pin);
  delay(10);
  data.value_y = Jstick(Jstick_y_pin);
  delay(10);
  radio.write(&data, sizeof(Value));

  //Serial.print("x: "); Serial.print(data.value_x);
  //Serial.print(", y: "); Serial.println(data.value_y);
}

uint16_t Jstick(int Jstick_Pin){
  return analogRead(Jstick_Pin);
}

// TB6612FNG right side & ESP32-WROOM-32D DEVKIT_C V4 left side pin order
const int PIN_PWMA = 32;  
const int PIN_AIN1 = 33;
const int PIN_AIN2 = 25;
const int PIN_STBY = 26;
const int PIN_BIN1 = 27;
const int PIN_BIN2 = 14;
const int PIN_PWMB = 12;

/*
// TB6612FNG right side & ESP32-WROOM-32 left side pin order
const int PIN_PWMA = 33;  
const int PIN_AIN1 = 25;
const int PIN_AIN2 = 26;
const int PIN_STBY = 27;
const int PIN_BIN1 = 14;
const int PIN_BIN2 = 12;
const int PIN_PWMB = 13;
*/

// PWM Channel setup
const int CH_PWMA = 0;
const int CH_PWMB = 1;
// PWM frequency and bit resolution setup
const int pwmFrequency = 10000;  // Hz
const int bitResolution = 8;     // pwm value: 0~255

const int PIN_LED = 2;

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  pinMode(PIN_LED, OUTPUT);

  // tb6612fng setting
  pinMode(PIN_STBY,OUTPUT);
  pinMode(PIN_AIN1,OUTPUT);
  pinMode(PIN_AIN2,OUTPUT);
  pinMode(PIN_PWMA,OUTPUT);
  pinMode(PIN_BIN1,OUTPUT);
  pinMode(PIN_BIN2,OUTPUT);
  pinMode(PIN_PWMB,OUTPUT);

   // motor output(PWM) setting (channel, frequency, bit)
  ledcSetup(CH_PWMA, pwmFrequency, bitResolution);
  ledcSetup(CH_PWMB, pwmFrequency, bitResolution);
  ledcAttachPin(PIN_PWMA, CH_PWMA);
  ledcAttachPin(PIN_PWMB, CH_PWMB);
  
  // channel setting (GPIO pin, channel)
  // if STBY(CH0) = LOW, Standby states.
  // if STBY(CH0) = HIGH, and INA1&INA2 have different value
  // and PWMA has some value, CW or CCW rotation is made.

  // Channel      CW   CCW  Stanby Stop Brake1 Brake2 Brake3
  // PIN_STBY (0) HIGH HIGH LOW    HIGH  HIGH  HIGH   HIGH
  // PIN_INA1 (1) HIGH LOW  *      LOW   HIGH  LOW    HIGH
  // PIN_INA2 (2) LOW  HIGH *      LOW   HIGH  HIGH   LOW
  // CH_PWMA  (3) PWM  PWM  *      HIGH  *     LOW    LOW  
}

void loop() {
 
  // Motor A : CW
  digitalWrite(PIN_STBY, HIGH); 	// STBY
  digitalWrite(PIN_AIN1, HIGH); 	// AIN1
  digitalWrite(PIN_AIN2, LOW);   	// AIN2
  ledcWrite(CH_PWMA, 255); 			// PWMA
  // Motor B : CW
  digitalWrite(PIN_BIN1, HIGH); 	// BIN1
  digitalWrite(PIN_BIN2, LOW);   	// BIN2
  ledcWrite(CH_PWMB, 255); 			// PWMB
  delay(3000);

  // Motor : Stand-by
  digitalWrite(PIN_STBY, LOW); 		// STBY
  //delay(1000);

  //LED Blink
  digitalWrite(ledPin, LOW);
  delay(3500);
  digitalWrite(ledPin, HIGH);
  delay(500);
  digitalWrite(ledPin, LOW);
  
  // Motor A : CCW
  digitalWrite(PIN_STBY, HIGH); 	// STBY
  digitalWrite(PIN_AIN1, LOW); 		// AIN1
  digitalWrite(PIN_AIN2, HIGH);   	// AIN2
  ledcWrite(CH_PWMA, 255); 			// PWMA
  // Motor B : CCW
  digitalWrite(PIN_BIN1, LOW); 		// BIN1
  digitalWrite(PIN_BIN2, HIGH);   	// BIN2
  ledcWrite(CH_PWMB, 255); 			// PWMB
  delay(3000);

  // Motor : Stand-by
  digitalWrite(PIN_STBY, LOW); 		// STBY
  //delay(1000);

  //LED Blink
  digitalWrite(PIN_LED, LOW);
  delay(3500);
  digitalWrite(PIN_LED, HIGH);
  delay(500);
  digitalWrite(PIN_LED, LOW);
}

const int AIN2 = 25;
const int AIN1 = 26;

const int AIN2 = 26;
const int AIN1 = 25;

// TB6612FNG right side & ESP32-WROOM-32D DEVKIT_C V4 left side pin order
const int PIN_PWMA = 32;  
const int PIN_AIN1 = 33;
const int PIN_AIN2 = 25;
const int PIN_STBY = 26;
const int PIN_BIN1 = 27;
const int PIN_BIN2 = 14;
const int PIN_PWMB = 12;

/*
// TB6612FNG right side & ESP32-WROOM-32 left side pin order
const int PIN_PWMA = 33;  
const int PIN_AIN1 = 25;
const int PIN_AIN2 = 26;
const int PIN_STBY = 27;
const int PIN_BIN1 = 14;
const int PIN_BIN2 = 12;
const int PIN_PWMB = 13;
*/

// PWM Channel setup
const int CH_PWMA = 0;
const int CH_PWMB = 1;
// PWM frequency and bit resolution setup
const int pwmFrequency = 10000;  // Hz
const int bitResolution = 8;     // pwm value: 0~255

const int PIN_LED = 2;


void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  pinMode(PIN_LED, OUTPUT);

  // tb6612fng setting
  pinMode(PIN_STBY,OUTPUT);
  pinMode(PIN_AIN1,OUTPUT);
  pinMode(PIN_AIN2,OUTPUT);
  pinMode(PIN_PWMA,OUTPUT);
  pinMode(PIN_BIN1,OUTPUT);
  pinMode(PIN_BIN2,OUTPUT);
  pinMode(PIN_PWMB,OUTPUT);

   // motor output(PWM) setting (channel, frequency, bit)
  ledcSetup(CH_PWMA, pwmFrequency, bitResolution);
  ledcSetup(CH_PWMB, pwmFrequency, bitResolution);
  ledcAttachPin(PIN_PWMA, CH_PWMA);
  ledcAttachPin(PIN_PWMB, CH_PWMB);
  
  // channel setting (GPIO pin, channel)
  // if STBY(CH0) = LOW, Standby states.
  // if STBY(CH0) = HIGH, and INA1&INA2 have different value
  // and PWMA has some value, CW or CCW rotation is made.

  // Channel      CW   CCW  Stanby Stop Brake1 Brake2 Brake3
  // PIN_STBY (0) HIGH HIGH LOW    HIGH  HIGH  HIGH   HIGH
  // PIN_INA1 (1) HIGH LOW  *      LOW   HIGH  LOW    HIGH
  // PIN_INA2 (2) LOW  HIGH *      LOW   HIGH  HIGH   LOW
  // CH_PWMA  (3) PWM  PWM  *      HIGH  *     LOW    LOW  
}


void loop() {
  
  move(1, 128, 0);               // motor A(right wheels), half speed, moving forward
  move(2, 255, 0);               // motor B(left wheels), full speed, moving forward
  delay(3000);

  stop();						 // Motor : Stand-by
  //delay(1000);

  //LED Blink
  digitalWrite(ledPin, LOW);
  delay(3500);
  digitalWrite(ledPin, HIGH);
  delay(500);
  digitalWrite(ledPin, LOW);

  move(1, 255, 1);               // motor A(right wheels), full speed, moving backward
  move(2, 128, 1);               // motor B(left wheels), half speed, moving backward
  delay(3000);

  stop();						 // Motor : Stand-by
  //delay(1000);

  //LED Blink
  digitalWrite(ledPin, LOW);
  delay(3500);
  digitalWrite(ledPin, HIGH);
  delay(500);
  digitalWrite(ledPin, LOW);
}

void move(int motor, int speed, int direction) {   //Move specific motor at speed and direction
//motor: A(Right) -> 1, B(Left) -> 2
//speed: 0 is off, and 255 is full speed
//direction: 0 clockwise, 1 counter-clockwise

  digitalWrite(PIN_STBY, HIGH);         // move

  boolean inPin1 = HIGH;            	// Defalut(direction=0) - Clockwise
  boolean inPin2 = LOW;

  if(direction == 1) {             		// Count-clockwise
    inPin1 = LOW;
    inPin2 = HIGH;
  }

  if(motor == 1){                 		// if motor == 1, right wheel
    digitalWrite(PIN_AIN1, inPin1);
    digitalWrite(PIN_AIN2, inPin2);
    ledcWrite(CH_PWMA, speed);
  } else {                         		// if motor is not 1, left wheel
    digitalWrite(PIN_BIN1, inPin1);
    digitalWrite(PIN_BIN2, inPin2);
    ledcWrite(CH_PWMB, speed);
  }
}

void stop() {
  digitalWrite(PIN_STBY, LOW);          // stand-by = stop
}

//Libraries for NRF24L01+ module.
#include <SPI.h>
#include <nRF24L01.h>
#include <RF24.h>
 
//RF24 object with two pins defined with arguments. CE: 4, CSN: 5
RF24 radio(4, 5);
 
//Address of the pipe. 40 bit long, you can choose this freely.
//Remember to use different address in different projects.
long long address = 0x1234ABCDEFLL;

int count = 0;
char stext[32] = "";
int spos = 0;
char rtext[32] = "";
int rpos = 0;

void sendText(char * text, int tlen)
{
  radio.stopListening();
  radio.openWritingPipe(address);
  radio.write(stext, tlen);

  Serial.print("SEND: ");
  Serial.println(text);    

  memset(stext, 0x00, 32);

  radio.openReadingPipe(0, address);
  radio.startListening();
}

void setup() {
  //Start the radio
  Serial.begin(115200);
  radio.begin();

  //Open reading pipe with given address and start listening for incoming data
  radio.openReadingPipe(0, address);
  radio.setPALevel(RF24_PA_MIN); 
  radio.startListening();
}
 
void loop() {
  while(0 < Serial.available()) {
    stext[spos] = Serial.read();
    
    if(stext[spos] == 0x0a) {
      sendText(stext, spos);
      spos = 0;
    }
    else {
      spos += 1;
    }
  }

  if (radio.available()) {
    while (radio.available()) {
      radio.read(rtext, 32);
    }
    Serial.print("RECV: ");
    Serial.println(rtext);
  }
}

//Libraries for NRF24L01+ module.
#include <SPI.h>
#include <nRF24L01.h>
#include <RF24.h>
 
//RF24 object with two pins defined with arguments. CE: 4, CSN: 5
RF24 radio(4, 5);
 
//Address of the pipe. 40 bit long, you can choose this freely.
//Remember to use different address in different projects.
long long address = 0x1234ABCDEFLL;

String username = "";
String dataInput;
char dataToSend[32];
char dataReceived[32];

void setup() {
  //Start the radio
  Serial.begin(115200);
  delay(2000);
  Serial.println("Enter username: ");
  radio.begin();
  radio.setRetries(3, 5);
  //Open reading pipe with given address and start listening for incoming data
  radio.openWritingPipe(address);
  radio.openReadingPipe(0, address);
  radio.setPALevel(RF24_PA_MIN); 
}

void loop() {

  //set username
  while(username == "") {
    if(Serial.available()) {
      username = Serial.readStringUntil('\n');
      Serial.print("Welcome ");
      Serial.println(username);
    }
  }

  radio.startListening();

  if(radio.available()) {
    radio.read(&dataReceived, sizeof(dataReceived));
    Serial.println(dataReceived);
  }
  
  if(Serial.available()) {
    radio.stopListening();

    dataInput = "[" + username + "] " + Serial.readStringUntil('\n');
    Serial.println(dataInput);
    dataInput.toCharArray(dataToSend, 32);

    radio.write(&dataToSend, sizeof(dataToSend));
  }
}

#include <SPI.h>
#include <RF24.h>
RF24 radio(4 ,5);

long long address = 0x1234ABCDEFLL;

struct Value{
  uint16_t value_x;
  uint16_t value_y;
};

Value data;


void setup() {
  Serial.begin(115200);
  radio.begin();
  radio.openReadingPipe(1, address);
  radio.setPALevel(RF24_PA_LOW);
  radio.startListening();
}

unsigned long lastRecvTime = 0;

void recvData(){
  while( radio.available()){
    radio.read(&data, sizeof(Value));
    lastRecvTime = millis();  
  }
}

void loop() {
    unsigned long now = millis();
  
  if( now - lastRecvTime > 1000){
	recvData();
  }

  Serial.print("x: "); Serial.print(data.value_x);
  Serial.print(", y: "); Serial.println(data.value_y); 
}

#include <SPI.h>
#include <RF24.h>
RF24 radio(4 ,5);

long long address = 0x1234ABCDEFLL;

struct Value{
  uint16_t value_x;
  uint16_t value_y;
};

Value data;

//Servo motor library for ESP32
#include <ESP32Servo.h>

Servo servo_x;  // create servo object to control a servo
Servo servo_y;  // 16 servo objects can be created on the ESP32

int angle_x = 90;
int angle_y = 90;
int i = 0;
int center_x = 0;
int center_y = 0;
int ref_xl, ref_xr, ref_ya, ref_yb;    // Servo rotation reference value 

// for no interference, each joysticks must be used with other ADC! (* not recommended)
// one joystick use ADC1: GPIO 36,39,32,33,34,35
// the other must use ADC2: GPIO 4,0*,2,15,13,12,14,27,25,26
int servo_xPin = 15;
int servo_yPin = 2;

// RecvTime
unsigned long lastRecvTime = 0;

void centerData(){
  while(i < 100) {
    if(radio.available()){
      radio.read(&data, sizeof(Value));
      center_x = center_x + data.value_x;
      center_y = center_y + data.value_y;
      i++;
    }
  }
  center_x = center_x / 100;         // x축 조이스틱 센터값
  center_y = center_y / 100;         // y축 조이스틱 센터값

  ref_xl = center_x / 2;             // 좌회전 동작 기준값
  ref_xr = (4095 + center_x) / 2;    // 우회전 동작 기준값
  ref_ya = (4095 + center_y) / 2;    // accelator 동작 기준값
  ref_yb = center_y / 2;             // brake 동작 기준값
}

void recvData(){
  while(radio.available()){
    radio.read(&data, sizeof(Value));
    lastRecvTime = millis();  
  }
}

void setup() {
  Serial.begin(115200);
  
  radio.begin();
  radio.openReadingPipe(1, address);
  radio.setPALevel(RF24_PA_LOW);
  radio.startListening();
  
  // Allow allocation of all timers
  ESP32PWM::allocateTimer(0);
  ESP32PWM::allocateTimer(1);
  ESP32PWM::allocateTimer(2);
  ESP32PWM::allocateTimer(3);
  servo_x.setPeriodHertz(50);    // standard 50 hz servo
  servo_y.setPeriodHertz(50);
  servo_x.attach(servo_xPin, 500, 2500); // attaches the servo on pin 25 & 32 to the servo object
  servo_y.attach(servo_yPin, 500, 2500);
  // using default min/max of 1000us and 2000us
  // different servos may require different min/max settings
  // for an accurate 0 to 180 sweep

  // center value
  centerData();
}

void loop() {
  unsigned long now = millis();
  
  if( now - lastRecvTime > 5){    // 5ms 마다 서보출력
    recvData();
    rotate_xy();
  }
}

void rotate_xy() {
  //Serial.print("x: "); Serial.print(data.value_x);
  //Serial.print(", y: "); Serial.println(data.value_y);

  //x축 서보 구동 조건 설정
  if(data.value_x < ref_xl) {
    if(angle_x > 0) {
      angle_x--;
    }
    else {
      angle_x = 0;
    }
  }
  else if(data.value_x > ref_xr) {
    if(angle_x < 180) {
      angle_x++;
    }
    else {
      angle_x = 180;
    }
  }
  else {
    if(angle_x < 90) {
      angle_x++;
    }
    else if(angle_x > 90) {
      angle_x--;
    }
    else {
      angle_x = 90;
    }
  }

 //y축 서보 구동 조건 설정
 if(data.value_y < ref_yb) {
    if(angle_y > 0) {
      angle_y--;
    }
    else {
      angle_y = 0;
    }
  }
  else if(data.value_y > ref_ya) {
    if(angle_y < 180) {
      angle_y++;
    }
    else {
      angle_y = 180;
    }
  }
  else {
    if(angle_y < 90) {
      angle_y++;
    }
    else if(angle_y > 90) {
      angle_y--;
    }
    else {
      angle_y = 90;
    }
  }
  
  servo_x.write(angle_x);
  servo_y.write(angle_y);
  delay(5);                    // delay를 더 줄이면 서보 모터 기어에 무리가 감
}