#include <Servo.h>
int YawInput = A0;
int ThrottleInput = A1;
int ch3 = A2;
int ch4 = A3;
int lmwpin = 3;
int rmwpin = 5;
int lbwpin = 6;
int rbwpin = 9;
int rudderpin = 10;
int ThrottleOutput = 11;
Servo Motor;
Servo lmw;
Servo rmw;
Servo lbw;
Servo rbw;
Servo rudder;
void setup()
{
pinMode(YawInput, INPUT);
pinMode(ThrottleInput, INPUT);
pinMode(ch3, INPUT);
pinMode(ch4, INPUT);
pinMode(lmwpin, OUTPUT);
pinMode(rmwpin, OUTPUT);
pinMode(lbwpin, OUTPUT);
pinMode(rbwpin, OUTPUT);
pinMode(rudderpin, OUTPUT);
Motor.attach(11, 1000, 2000);
lmw.attach(3);
rmw.attach(5);
lbw.attach(6);
rbw.attach(9);
rudder.attach(10);
Serial.begin(115200);
digitalWrite(12, HIGH);
delay(200);
digitalWrite(12, LOW);
delay(5000);
Motor.write(180);
delay(5000);
Motor.write(0);
delay(10000);
digitalWrite(12, HIGH);
delay(200);
digitalWrite(12, LOW);
lmw.write(10);
rmw.write(10);
lbw.write(10);
rbw.write(10);
delay(1750);
lmw.write(170);
rmw.write(170);
lbw.write(170);
rbw.write(170);
delay(1750);
lmw.write(10);
rmw.write(10);
lbw.write(10);
rbw.write(10);
delay(1750);
lmw.write(170);
rmw.write(170);
lbw.write(170);
rbw.write(170);
delay(1750);
lmw.write(90);
rmw.write(90);
lbw.write(90);
rbw.write(90);
delay(1750);
digitalWrite(12, HIGH);
delay(200);
digitalWrite(12, LOW);
rudder.write(10);
delay(1500);
rudder.write(170);
delay(1500);
rudder.write(90);
digitalWrite(12, HIGH);
delay(200);
digitalWrite(12, LOW);
delay(200);
digitalWrite(12, HIGH);
delay(200);
digitalWrite(12, LOW);
delay(200);
digitalWrite(12, HIGH);
delay(200);
digitalWrite(12, LOW);
}
void loop()
{
int ReadYaw = pulseIn(YawInput, HIGH);
int ReadThrottle = pulseIn(ThrottleInput, HIGH);
int Readch3 = pulseIn(ch3, HIGH);
int Readch4 = pulseIn(ch4, HIGH);
int YawMap = map(ReadYaw, 870, 1915, 10, 170);
Serial.println(YawMap);
int Throttle = map(ReadThrottle, 885, 1915, 0, 170);
if (Readch3 > 1300)
{
Serial.print("Roll - ");
if (YawMap < 85)
{
Serial.println("Roll left");
lmw.write(180-YawMap);
rmw.write(180-YawMap);
}
else if (YawMap < 95)
{
Serial.println("Roll stable");
lmw.write(90);
rmw.write(90);
}
else
{
Serial.println("Roll right");
lmw.write(180-YawMap);
rmw.write(180-YawMap);
}
Serial.print("Pitch - ");
if (Throttle < 85)
{
Serial.println("Pitch Down");
lbw.write(180-Throttle);
rbw.write(Throttle);
}
else if (Throttle < 95)
{
Serial.println("Pitch stable");
lbw.write(90);
rbw.write(90);
}
else
{
Serial.println("Pitch up");
lbw.write(180-Throttle);
rbw.write(Throttle);
}
}
else{}
if(Readch4 > 1300)
{
Motor.write(0);
}
else
{}
if (Readch3 < 1300)
{
Serial.print("Yaw - ");
if (YawMap < 85)
{
Serial.println("Yaw left");
rudder.write(180-YawMap);
}
else if (YawMap < 95)
{
Serial.println("Yaw stable");
rudder.write(90);
}
else
{
Serial.println("Yaw right");
rudder.write(180-YawMap);
}
if(Throttle > 10 && Throttle < 170)
{
Serial.print("Throttle - ");
if (Throttle < 85)
{
Serial.println("Throttle Down");
Motor.write(Throttle);
}
else if (Throttle < 95)
{
Serial.println("Throttle stable");
Motor.write(90);
}
else
{
Serial.println("Throttle up");
Motor.write(Throttle);
}
}
else
{
Motor.write(0);
digitalWrite(12, HIGH);
delay(200);
digitalWrite(12, LOW);
}
}
else{}
delay(1);
}