#include <Servo.h> //Servo 라이브러리 생성
#include <IBusBM.h> //IBusBM 라이브러리 생성

#define motorW1_IN1 3 //모터드라이버의 motorW1_IN1부분을 3번에 연결했다고 알려줌
#define motorW1_IN2 4 //모터드라이버의 motorW1_IN2부분을 4번에 연결했다고 알려줌
#define motorW2_IN1 5 //모터드라이버의 motorW2_IN1부분을 5번에 연결했다고 알려줌
#define motorW2_IN2 6 //모터드라이버의 motorW2_IN2부분을 6번에 연결했다고 알려줌

Servo servoW1; //서보모터 servo1을 지정함
Servo servoW3; //서보모터 servo3을 지정함
Servo servoW4; //서보모터 servo4을 지정함
Servo servoW6; //서보모터 servo6을 지정함
IBusBM IBus; //IBusBm IBus를 지정함 
IBusBM IBusSensor; //IBusBm IBusSensor를 지정함

int ch0, ch1, ch2, ch3, ch6 = 0; //변수 ch0, ch1, ch2, ch3, ch6의 값을 0으로 정함
int r22=110; //변수 r22의 값을 110으로 정함
int r23=92; //변수 r23의 값을 92으로 정함
int r24=100; //변수 r24의 값을 100으로 정함
int r25=70; //변수 r25의 값을 70으로 정함
float speed1PWM= 0; //변수 speed1PWM을 0으로 정함

void setup() {

Serial.begin(115200); //시리얼통신을 115200으로 개봉함 
IBus.begin(Serial1, IBUSBM_NOTIMER); // Servo iBUS
IBusSensor.begin(Serial2, IBUSBM_NOTIMER); // Sensor iBUS
IBusSensor.addSensor(IBUSS_INTV); // add voltage sensor
servoW1.attach(22); //서보모터 servo1을 22번 핀에 연결했다고 알려줌
servoW3.attach(23); //서보모터 servo3을 23번 핀에 연결했다고 알려줌
servoW4.attach(24); //서보모터 servo4을 24번 핀에 연결했다고 알려줌
servoW6.attach(25); //서보모터 servo6을 25번 핀에 연결했다고 알려줌

}

void loop() {

IBus.loop();
ch0 = IBus.readChannel(0); //채널 1  좌우방향
ch1 = IBus.readChannel(1); //채널 2  전진후진
ch2 = IBus.readChannel(2); //채널 3  현재 기능 없음 
ch3 = IBus.readChannel(3); //채널 4  현재 기능 없음
ch6 = IBus.readChannel(6); //채널 7  현재 기능 없음

servoW1.write(r22); //서보모터 servoW1의 각도를 r22 변수값으로 정함
servoW3.write(r23); //서보모터 servoW1의 각도를 r23 변수값으로 정함
servoW4.write(r24); //서보모터 servoW1의 각도를 r24 변수값으로 정함
servoW6.write(r25); //서보모터 servoW1의 각도를 r25 변수값으로 정함

if (ch0 > 1600)//우회전
{

  int R=map(ch1,1600,2000,30,50); //변수 R의 값을 ch1 변수값에서 나오는 1600~2000의 
값을 30~50의 값으로 변환한 값으로 정함 
  servoW1.write(r22+40); //서보모터 servoW1의 각도를 r22 변수값+40으로 정함
  servoW3.write(r23-40); //서보모터 servoW1의 각도를 r23 변수값-40으로 정함

  servoW4.write(r24-40); //서보모터 servoW1의 각도를 r24 변수값-40으로 정함
  servoW6.write(r25+40); //서보모터 servoW1의 각도를 r25 변수값+40으로 정함
  delay(100);

 analogWrite(motorW1_IN1, 255); 
 analogWrite(motorW1_IN2, 255);
 analogWrite(motorW2_IN1, 0);
 analogWrite(motorW2_IN2, 0);

if (ch1 > 1600) //전진
{
servoW1.write(r22); //서보모터 servoW1의 각도를 r22 변수값으로 정함
servoW3.write(r23); //서보모터 servoW1의 각도를 r23 변수값으로 정함
servoW4.write(r24); //서보모터 servoW1의 각도를 r24 변수값으로 정함
servoW6.write(r25); //서보모터 servoW1의 각도를 r25 변수값으로 정함
speed1PWM=map(ch1,1600,2000,0,255);

 analogWrite(motorW1_IN1, 0); 
 analogWrite(motorW1_IN2, speed1PWM);
 analogWrite(motorW2_IN1, speed1PWM);
 analogWrite(motorW2_IN2, 0);

}

else if (ch1 < 1500) //후진
{
servoW1.write(r22); //서보모터 servoW1의 각도를 r22 변수값으로 정함
servoW3.write(r23); //서보모터 servoW1의 각도를 r23 변수값으로 정함
servoW4.write(r24); //서보모터 servoW1의 각도를 r24 변수값으로 정함
servoW6.write(r25); //서보모터 servoW1의 각도를 r25 변수값으로 정함

  speed1PWM=map(ch1,1500,1000,0,255);
  analogWrite(motorW1_IN1, speed1PWM);
  analogWrite(motorW1_IN2, 0);
  analogWrite(motorW2_IN1, 0);
  analogWrite(motorW2_IN2, speed1PWM);

}
else
{
servoW1.write(r22); //서보모터 servoW1의 각도를 r22 변수값으로 정함
servoW3.write(r23); //서보모터 servoW1의 각도를 r23 변수값으로 정함
servoW4.write(r24); //서보모터 servoW1의 각도를 r24 변수값으로 정함
servoW6.write(r25); //서보모터 servoW1의 각도를 r25 변수값으로 정함

  analogWrite(motorW1_IN1, 0);
  analogWrite(motorW1_IN2, 0);
  analogWrite(motorW2_IN1, 0);
 analogWrite(motorW2_IN2, 0);

}
}

else if (ch0 <1300)//좌회전
{

  int R=map(ch1,1600,2000,30,50); //변수 R의 값을 ch1 변수값에서 나오는 1600~2000의 
값을 30~50의 값으로 변환한 값으로 정함 
  servoW1.write(r22+40); //서보모터 servoW1의 각도를 r22 변수값+40으로 정함
  servoW3.write(r23-40); //서보모터 servoW1의 각도를 r23 변수값-40으로 정함

  servoW4.write(r24-40); //서보모터 servoW1의 각도를 r24 변수값-40으로 정함
  servoW6.write(r25+40); //서보모터 servoW1의 각도를 r25 변수값+40으로 정함
  delay(100);

if (ch1 > 1600) //전진
{
servoW1.write(r22); //서보모터 servoW1의 각도를 r22 변수값으로 정함
servoW3.write(r23); //서보모터 servoW1의 각도를 r23 변수값으로 정함
servoW4.write(r24); //서보모터 servoW1의 각도를 r24 변수값으로 정함
servoW6.write(r25); //서보모터 servoW1의 각도를 r25 변수값으로 정함
speed1PWM=map(ch1,1600,2000,0,255);

 analogWrite(motorW1_IN1, 0);
 analogWrite(motorW1_IN2, speed1PWM);
 analogWrite(motorW2_IN1, speed1PWM);
 analogWrite(motorW2_IN2, 0);

}

else if (ch1 < 1500) //후진
{
servoW1.write(r22); //서보모터 servoW1의 각도를 r22 변수값으로 정함
servoW3.write(r23); //서보모터 servoW1의 각도를 r23 변수값으로 정함
servoW4.write(r24); //서보모터 servoW1의 각도를 r24 변수값으로 정함
servoW6.write(r25); //서보모터 servoW1의 각도를 r25 변수값으로 정함

  speed1PWM=map(ch1,1500,1000,0,255);
  analogWrite(motorW1_IN1, speed1PWM);
  analogWrite(motorW1_IN2, 0);
  analogWrite(motorW2_IN1, 0);
  analogWrite(motorW2_IN2, speed1PWM);

}
else
{
servoW1.write(r22); //서보모터 servoW1의 각도를 r22 변수값으로 정함
servoW3.write(r23); //서보모터 servoW1의 각도를 r23 변수값으로 정함
servoW4.write(r24); //서보모터 servoW1의 각도를 r24 변수값으로 정함
servoW6.write(r25); //서보모터 servoW1의 각도를 r25 변수값으로 정함

  analogWrite(motorW1_IN1, 0);
  analogWrite(motorW1_IN2, 0);
  analogWrite(motorW2_IN1, 0);
 analogWrite(motorW2_IN2, 0);

}

}

else if (ch1 > 1600) //전진
{
servoW1.write(r22); //서보모터 servoW1의 각도를 r22 변수값으로 정함
servoW3.write(r23); //서보모터 servoW1의 각도를 r23 변수값으로 정함
servoW4.write(r24); //서보모터 servoW1의 각도를 r24 변수값으로 정함
servoW6.write(r25); //서보모터 servoW1의 각도를 r25 변수값으로 정함
speed1PWM=map(ch1,1600,2000,0,255);

 analogWrite(motorW1_IN1, 0);
 analogWrite(motorW1_IN2, speed1PWM);
 analogWrite(motorW2_IN1, 0);
 analogWrite(motorW2_IN2, speed1PWM);

}

else if (ch1 < 1500) //후진
{
servoW1.write(r22); //서보모터 servoW1의 각도를 r22 변수값으로 정함
servoW3.write(r23); //서보모터 servoW1의 각도를 r23 변수값으로 정함
servoW4.write(r24); //서보모터 servoW1의 각도를 r24 변수값으로 정함
servoW6.write(r25); //서보모터 servoW1의 각도를 r25 변수값으로 정함

  speed1PWM=map(ch1,1500,1000,0,255);
  analogWrite(motorW1_IN1, speed1PWM);
  analogWrite(motorW1_IN2, 0);
  analogWrite(motorW2_IN1, speed1PWM);
  analogWrite(motorW2_IN2, 0);

}
else
{
servoW1.write(r22); //서보모터 servoW1의 각도를 r22 변수값으로 정함
servoW3.write(r23); //서보모터 servoW1의 각도를 r23 변수값으로 정함
servoW4.write(r24); //서보모터 servoW1의 각도를 r24 변수값으로 정함
servoW6.write(r25); //서보모터 servoW1의 각도를 r25 변수값으로 정함

  analogWrite(motorW1_IN1, 0);
  analogWrite(motorW1_IN2, 0);
  analogWrite(motorW2_IN1, 0);
 analogWrite(motorW2_IN2, 0);

}

}