int elevator = 2;
int rudder = 3;
int elev_signal = 0;
int rudd_signal = 0;
int servo_1 = 4;
int servo_2 = 5;
int out_1 = 0;
int out_2 = 0;
unsigned long timer;
unsigned long x = 0;
unsigned long frame;
void setup()
{
Serial.begin(9600);
pinMode(elevator, INPUT);
pinMode(rudder, INPUT);
pinMode(servo_1, OUTPUT);
pinMode(servo_2, OUTPUT);
}
void loop()
{
elev_signal = pulseIn(elevator, HIGH, 25000);
rudd_signal = pulseIn(rudder, HIGH, 25000);
elev_signal = elev_signal - 1000;
rudd_signal = rudd_signal - 1000;
elev_signal = elev_signal / 2;
rudd_signal = rudd_signal / 2;
out_1 = elev_signal + rudd_signal + 1000;
constrain(out_1, 800, 2200);
out_2 = rudd_signal - elev_signal + 1500;
constrain(out_2, 800, 2200);
Serial.print("OUT2 = ");
Serial.println(out_2);
servoout();
}
void servoout()
{
digitalWrite(servo_1, LOW);
digitalWrite(servo_2, LOW);
timer = micros();
frame = timer;
digitalWrite(servo_1, HIGH);
while((micros() - timer) < out_1)
{
}
digitalWrite(servo_1, LOW);
digitalWrite(servo_2, HIGH);
timer = micros();
while((micros() - timer) < out_2)
{
}
digitalWrite(servo_2, LOW);
while((micros() - frame) < 20000)
{
}
}