Arduino CODE for SFODragonfly90-4S
New CODE 2023/05/19 2024/03/08 :2024/10/01
4 Servos Flap System for SFODragpnfly90-4S
Front Servo
Servo right
--D5 pin
Servo left
--D6 pin
Rear Servo
Servo right 2nd
--D8 pin
Servo left 2nd
--D9 pin
Ground -GND pin
6V RAW pin-Lipo2cell 70-120mAh –DC Stepdown
converter –6V to Raw pin
DC converter 3A
Adjustable Output 6.2V at 14/October/2021
Mini
DC-DC Adjustable Step Down Power Supply Module
DC converter 2A 4-36V to
3.3V/5V/6V/9V/12V Converter Step Down Voltage Regulator Power Module - 3.3V—use
6V
https://www.aliexpress.com/item/1005002458992557.html?spm=a2g0s.9042311.0.0.69a24c4dKCqDnP
Servo: BLUEARROW AF D43S-6.0-MG Micro Metal Gear
Digital Servo
RX: OrangeRx R616XN DSM2
/ DSMX6CH CPPM
TX: Jumper T8SG
5V to Receiver
6V to
Added ch7 RearWingAngleTrim(Ver3.0)
Arduino CODE
Nikkilae/PPM-reader https://github.com/Nikkilae/PPM-reader
Old Version only Front wing move by Ailron channel --ch1
//210916 15-4th Code 4servo
Rudder ElevatorUP FlapAmpUP delaytime control PPMRX Ch1-4
writeMicroseconds(1000-2000uS) use 5V3A FrontServoLate
RearServo Ailron act only Front servo by K.Kakuta
#include <Servo.h>
#include <PPMReader.h>
//#include <InterruptHandler.h>
//#define DOPRINTS // if you print on PC
screen Data, delete”//”
// In use of Flap system,cut
"Serial.print "(put ”//”)
//and write code to Arduino board.
int interruptPin = 2;
int channelAmount = 6;
PPMReader ppm(interruptPin, channelAmount);
int servo_right_pin = 5;
int servo_left_pin = 6;
int servo_right2_pin = 8;
int servo_left2_pin = 9;
volatile int elevator = 0;
volatile int flapamp = 0;
volatile int delaytime = 100;// Servo speed
low-increase Servo speed high-decrease this.
// unit: micro second
volatile int delaytime2 =
100;
volatile int delaytime3=100;
// Servo speed low-increase Servo speed high-decrease this.
// unit: micro second
volatile int delaytime4 =
100;
volatile int ch3value = 1000;//Ch3
volatile int ch1value = 1500;//Ch1
volatile int ch2value = 1500;//Ch2
volatile int ch4value = 1500;//Ch4
volatile int ch5value = 1500;//Ch5
volatile int ch6value =
1500;//Ch6 Right LeverSwitch
static int servo_comm1 = 0;// Left or Right
Servo high point and low point
static int servo_comm2 = 0; // Left or
Right Servo high point and low point
static int servo_comm3 = 0;// Left or Right
Servo high point and low point no rudder
static int servo_comm4 = 0; // Left or
Right Servo high point and low point no rudder
volatile int rudder = 0;
float glide_deg = 0; // Gliding angle 0=0
degree 500=90degree
static float servo_zero1 = 0;//flap angle
adjust
static float servo_zero2 = 0; //flap angle
adjust
Servo servo_left, servo_right,
servo_right2, servo_left2; // create servo object to control a servo
void setup() {
Serial.begin(9600);
pinMode(servo_left_pin, OUTPUT);
pinMode(servo_right_pin, OUTPUT);
pinMode(servo_left2_pin, OUTPUT);
pinMode(servo_right2_pin, OUTPUT);
servo_left.attach(servo_left_pin);//output pin No
servo_right.attach(servo_right_pin); //output pin No
servo_left2.attach(servo_left2_pin);//output pin No
servo_right2.attach(servo_right2_pin); //output pin No
delay(2000);//Avoid abnormal positions at startup-wait 2 second until RX
starts220306
}
void loop() {
ch3value = ppm.rawChannelValue(3);//Ch3
ch1value = ppm.rawChannelValue(1);//Ch1
ch2value = ppm.rawChannelValue(2);//Ch2
ch4value = ppm.rawChannelValue(4);//Ch4
ch5value = ppm.rawChannelValue(5);//Ch5
ch6value = ppm.rawChannelValue(6);//Ch6
#ifdef DOPRINTS
Serial.print("ch3value ");Serial.print(ch3value);
Serial.print(",\t");
Serial.print("ch1value ");Serial.print(ch1value);
Serial.print(",\t");
Serial.print("ch2value ");Serial.print(ch2value);
Serial.print(",\t");
Serial.print("ch4value ");Serial.print(ch4value);
Serial.print(",\t");
Serial.print("ch5value ");Serial.print(ch5value);
Serial.println(",\t");
Serial.print("ch6value
");Serial.print(ch6value);
Serial.println(",\t");
#endif
rudder=(int)(ch1value-1500);//Ch1 Flap angle incline
elevator=(int)(ch2value-1500);//Ch2 Flap Angle
bilateral UP&Down
flapamp=(int)(ch4value-1500);//Ch4 Right and
left Flap angle difference
delaytime=(int)((ch5value-950)/5);//Ch5
Flapping frequency
delaytime2=(int)((ch6value-950)/10);//Ch6
Delay frontServo-RearServo
// you can change UP or Down direction by
your transmitter Reverse setting of each Channel
#ifdef DOPRINTS
//Serial.print("rudder");Serial.print(rudder);
//Serial.print(",\t");
//Serial.print("elevator");Serial.print(elevator);
//Serial.print(",\t");
//Serial.print("flapamp");Serial.print(flapamp);
//Serial.print(",\t");
//Serial.print("delaytime");Serial.print(delaytime);
//Serial.print(",\t");
//Serial.print("delaytime2");Serial.print(delaytime);
//Serial.print(",\t");
#endif
if
(ch3value > 1080) {
servo_comm1 = (int)( (ch3value -1000)/2+1500
+ rudder - elevator+ servo_zero1+ flapamp);
servo_comm2 = (int)(1000 + (2000 - ((ch3value
-1000)/2+1500)) + rudder + elevator- servo_zero2+flapamp);
servo_comm3 = (int)( (ch3value -1000)/2+1500 -
elevator+ servo_zero1+ flapamp);
servo_comm4 = (int)(1000 + (2000 - ((ch3value
-1000)/2+1500)) + elevator- servo_zero2+flapamp);
//
Serial.print("servo_comm1
");Serial.print(servo_comm1);
//
Serial.print(",\t");
//
Serial.print("servo_comm2
");Serial.print(servo_comm2);
//
Serial.print(",\t");
//
Serial.print("servo_comm3
");Serial.print(servo_comm3);
//Serial.print(",\t");
//
Serial.print("servo_comm4
");Serial.print(servo_comm4);
servo_left2.writeMicroseconds(servo_comm3);
// Rear left servo position
servo_right2.writeMicroseconds(servo_comm4);
// Rear right servo position
delaytime4 = delaytime *
delaytime2 / 200;
delay(delaytime4); //Wait
1second=1000mseconds
servo_left.writeMicroseconds(servo_comm1); //
Front left servo position
servo_right.writeMicroseconds(servo_comm2);
// Front right servo position
delaytime3
= delaytime- delaytime4;
delay(delaytime3); //Wait
1second=1000mseconds
servo_comm1 = (int)( (ch3value -1000)/2+1500
+ rudder + elevator+ servo_zero1-flapamp);
servo_comm2 = (int)(1000 + (2000 - ((ch3value -1000)/2+1500)) + rudder -
elevator- servo_zero2-flapamp); servo_comm3
= (int)( (ch3value -1000)/2+1500 + elevator+ servo_zero1-flapamp);
servo_comm4 = (int)(1000 + (2000 - ((ch3value
-1000)/2+1500)) - elevator- servo_zero2-flapamp);
servo_left2.writeMicroseconds(servo_comm4);
// Rear left servo position
servo_right2.writeMicroseconds(servo_comm3);
// Rear right servo position
delay(delaytime4); //Wait
1second=1000mseconds
servo_left.writeMicroseconds(servo_comm2); //
Front left servo position
servo_right.writeMicroseconds(servo_comm1);
// Front right servo position
#ifdef DOPRINTS
//Serial.print("servo_comm1");Serial.print(servo_comm1);
//Serial.print(",\t");
//Serial.print("servo_comm2");Serial.print(servo_comm2);
//Serial.println(",\t");
#endif
delay(delaytime3); //Wait 1second=1000mseconds
}
else{
servo_comm1=(int)(1500+rudder-elevator+glide_deg);
servo_comm2=(int)(1500+rudder+elevator-glide_deg);
servo_comm3=(int)(1500 -elevator+glide_deg);
servo_comm4=(int)(1500+elevator-glide_deg);
servo_left.writeMicroseconds(servo_comm1); // servo position in variable
'pos'
servo_right.writeMicroseconds(servo_comm2); // servo position in
variable 'pos'
servo_left2.writeMicroseconds(servo_comm3); //
servo position in variable 'pos'
servo_right2.writeMicroseconds(servo_comm4);
// servo position in variable 'pos'
#ifdef DOPRINTS
//Serial.print("servo_comm1");Serial.print(servo_comm1);
//Serial.print(",\t");
//Serial.print("servo_comm2");Serial.print(servo_comm2);
//Serial.println(",\t");
#endif
}
}
New CODE Version2 2023/01/07
Version2
dtとelapsed timeを使った、delay（この場合、羽ばたきの最初のみ送信機の情報を拾う）をつかわないCODEの製作
羽ばたきの最中も送信機の情報を拾うので、動作が機敏になる可能性がある。
I changed CODE that Flapping system work by
time not “delay” code.
In the previous CODE, transmitter
information was read only once at the beginning of flapping.
The new CODE constantly reads information
from the transmitter while flapping its wings, so it can respond to sudden
changes in Ornithopter's attitude.
Therefore, the Ornithopter can fly with
agile attitude changes even in strong winds.
When the voltage of the Lipo drops, the
flapping frequency increases, so it is necessary to cancel the flight and set
it again, or to set it again during the flight.
“delay” CODEを使わず、”時間”を使って羽ばたきを行うArduinoCODEに変更した。
以前のCODEでは送信機の情報は羽ばたきの最初に1回だけ読み込んでいた。
新しいCODEでは、羽ばたきの間常に送信機の情報を読んでいるので、Ornithopterの姿勢の急激な変化に対応ができる。
したがって、強風の中でも機敏に姿勢の変化をしてOrnithopterは飛行ができる。
Lipoの電圧が落ちてくると、羽ばたきの周波数が多くなるため、飛行を中止して設定しなおすか、飛行中に再設定する必要がある。
New Version Zip File
01/10/2024 Updated PPMReader to the latest version, V1.2.0
Nikkilae/PPM-reader https://github.com/Nikkilae/PPM-reader
dimag0g/PPM-reader https://www.arduinolibraries.info/libraries/ppm-reader
https://github.com/dimag0g/PPM-reader
SFO4servoFlapsystem Arduino CODE with PPMReader ZipFlile
//221208
Version2 added ch7 RearWingTrim 210916 15-4th Code 4servo Rudder ElevatorUP FlapAmpUP not
delaytime control PPMRX Ch1-4 writeMicroseconds(1000-2000uS) use 5V3A FrontServoLate RearServo Ailron act only
Front servo dt and elapsed time by K.Kakuta
#include
<Servo.h>
#include
<PPMReader.h>
//#include
<InterruptHandler.h> // 2022/01/27 Delete for more good move
//#define
DOPRINTS // if you print on PC screen Data, delete”//”
//
In use of Flap system,cut "Serial.print "(put ”//”)
//and
write code to Arduino board.
int
interruptPin = 2;
int
channelAmount = 8;
PPMReader
ppm(interruptPin, channelAmount);
int
servo_right_pin = 5;
int
servo_left_pin = 6;
int
servo_right2_pin = 8;
int
servo_left2_pin = 9;
volatile
int elevator = 0;
volatile
int flapamp = 0;
volatile
int rearwingtrim =0;
//volatile
int delaytime = 100;// Servo speed low-increase Servo speed high-decrease this.
// unit: micro second
//volatile
int delaytime2 = 100;
//volatile
int delaytime3=100; // Servo speed low-increase Servo speed high-decrease this.
// unit: micro second
//volatile
int delaytime4 = 100;
volatile float
delaytime = 100;// Servo speed low-increase Servo speed high-decrease this.
// unit: micro second
volatile float
delaytime2 = 100;
volatile float
delaytime3=100; // Servo speed low-increase Servo speed high-decrease this.
// unit: micro second
volatile float
delaytime4 = 100;
float elapsed_time =
0;
float dt;
unsigned long
current_time, prev_time;
volatile
int ch3value = 1000;//Ch3
volatile
int ch1value = 1500;//Ch1
volatile
int ch2value = 1500;//Ch2
volatile
int ch4value = 1500;//Ch4
volatile
int ch5value = 1500;//Ch5
volatile
int ch6value = 1500;//Ch6 Right LeverSwitch
volatile
int ch7value = 1500;//Ch7 rear wing trim
static
int servo_comm1 = 0;// Left or Right Servo high point and low point
static
int servo_comm2 = 0; // Left or Right Servo high point and low point
static
int servo_comm3 = 0;// Left or Right Servo high point and low point no rudder
static
int servo_comm4 = 0; // Left or Right Servo high point and low point no rudder
volatile
int rudder = 0;
float
glide_deg = 0; // Gliding angle 0=0 degree 500=90degree
static
float servo_zero1 = 0;//flap angle adjust
static
float servo_zero2 = 0; //flap angle adjust
Servo
servo_left, servo_right, servo_right2, servo_left2; // create servo object to
control a servo
void
setup() {
Serial.begin(9600);
pinMode(servo_left_pin, OUTPUT);
pinMode(servo_right_pin, OUTPUT);
pinMode(servo_left2_pin, OUTPUT);
pinMode(servo_right2_pin, OUTPUT);
servo_left.attach(servo_left_pin);//output
pin No
servo_right.attach(servo_right_pin); //output
pin No
servo_left2.attach(servo_left2_pin);//output
pin No
servo_right2.attach(servo_right2_pin);
//output pin No
delay(2000);//Avoid
abnormal positions at startup-wait 2 second until RX starts220306
}
void
loop() {
prev_time = current_time;
current_time = micros();
dt = (current_time - prev_time)/1000000.0;
elapsed_time = elapsed_time + dt; // total
time spent in the main loop since beginning one upstroke/downstroke
ch3value = ppm.rawChannelValue(3);//Ch3
ch1value = ppm.rawChannelValue(1);//Ch1
ch2value = ppm.rawChannelValue(2);//Ch2
ch4value = ppm.rawChannelValue(4);//Ch4
ch5value = ppm.rawChannelValue(5);//Ch5
ch6value
= ppm.rawChannelValue(6);//Ch6
ch7value
= ppm.rawChannelValue(7);//Ch7
#ifdef DOPRINTS
Serial.print("ch3value
");Serial.print(ch3value);
Serial.print(",\t");
Serial.print("ch1value
");Serial.print(ch1value);
Serial.print(",\t");
Serial.print("ch2value
");Serial.print(ch2value);
Serial.print(",\t");
Serial.print("ch4value
");Serial.print(ch4value);
Serial.print(",\t");
Serial.print("ch5value ");Serial.print(ch5value);
Serial.println(",\t");
Serial.print("ch6value
");Serial.print(ch6value);
Serial.println(",\t");
#endif
rudder=(int)(ch1value-1500);//Ch1 Flap angle incline
elevator=(int)(ch2value-1500);//Ch2 Flap Angle
bilateral UP&Down
flapamp=(int)(ch4value-1500);//Ch4 Right and
left Flap angle difference
//delaytime=(int)((ch5value-950)/5);//Ch5
Flapping frequency
//delaytime2=(int)((ch6value-950)/10);//Ch6
Delay frontServo-RearServo
//rearwingtrim=(int)(ch7value-1500);//Ch7
Right and left angle trim difference
delaytime=( ch5value -950)/5;//Ch5 Flapping frequency
delaytime2=( ch6value -950)/10;//Ch6 Delay frontServo-RearServo
rearwingtrim =( ch7value - 1500)/1000;//Ch7 Right and left angle trim difference
-0.5to0.5
delaytime4 =
delaytime * delaytime2 / 200;
delaytime3 =
delaytime- delaytime4;
//
you can change UP or Down direction by your transmitter Reverse setting of each
Channel
#ifdef DOPRINTS
//Serial.print("rudder");Serial.print(rudder);
//Serial.print(",\t");
//Serial.print("elevator");Serial.print(elevator);
//Serial.print(",\t");
//Serial.print("flapamp");Serial.print(flapamp);
//Serial.print(",\t");
//Serial.print("delaytime");Serial.print(delaytime);
//Serial.print(",\t");
//Serial.print("delaytime2");Serial.print(delaytime);
//Serial.print(",\t");
#endif
if (ch3value > 1080) {
if (elapsed_time < delaytime4/1000) {
servo_comm1 = (int)( (ch3value -1000)/2+1500
+ rudder - elevator+ servo_zero1+ flapamp);
servo_comm2 = (int)(1000 + (2000 - ((ch3value
-1000)/2+1500)) + rudder + elevator- servo_zero2+flapamp);
servo_comm3 = (int)( (ch3value -1000)/2+1500
- elevator + rearwingtrim + servo_zero1+ flapamp);
servo_comm4 = (int)(1000 + (2000 - ((ch3value
-1000)/2+1500)) + elevator + rearwingtrim - servo_zero2+flapamp);
// Serial.print("servo_comm1
");Serial.print(servo_comm1);
// Serial.print(",\t");
// Serial.print("servo_comm2
");Serial.print(servo_comm2);
// Serial.print(",\t");
// Serial.print("servo_comm3
");Serial.print(servo_comm3);
//Serial.print(",\t");
// Serial.print("servo_comm4
");Serial.print(servo_comm4);
servo_left2.writeMicroseconds(servo_comm3);
// Rear left servo position
servo_right2.writeMicroseconds(servo_comm4);
// Rear right servo position
}
//delaytime4
= delaytime * delaytime2 / 200;
//delay(delaytime4);
//Wait 1second=1000mseconds
if ((elapsed_time >
delaytime4/1000) && ( elapsed_time < (delaytime4 +
delaytime3)/1000)) {
servo_left.writeMicroseconds(servo_comm1); //
Front left servo position
servo_right.writeMicroseconds(servo_comm2);
// Front right servo position
}
//delaytime3
= delaytime- delaytime4;
//delay(delaytime3);
//Wait 1second=1000mseconds
if ((elapsed_time >
(delaytime4 + delaytime3)/1000) && (elapsed_time < (delaytime4 +
delaytime3 + delaytime4)/1000)) {
servo_comm1 = (int)( (ch3value -1000)/2+1500
+ rudder + elevator+ servo_zero1-flapamp);
servo_comm2 = (int)(1000 + (2000 - ((ch3value
-1000)/2+1500)) + rudder - elevator- servo_zero2-flapamp);
servo_comm3 = (int)( (ch3value -1000)/2+1500
+ elevator+ rearwingtrim + servo_zero1-flapamp);
servo_comm4 = (int)(1000 + (2000 - ((ch3value
-1000)/2+1500)) - elevator + rearwingtrim - servo_zero2-flapamp);
servo_left2.writeMicroseconds(servo_comm4);
// Rear left servo position
servo_right2.writeMicroseconds(servo_comm3);
// Rear right servo position
}
//delay(delaytime4);
//Wait 1second=1000mseconds
if
(elapsed_time > (delaytime4 + delaytime3 + delaytime4)/1000) {
servo_left.writeMicroseconds(servo_comm2); //
Front left servo position
servo_right.writeMicroseconds(servo_comm1);
// Front right servo position
#ifdef DOPRINTS
//Serial.print("servo_comm1");Serial.print(servo_comm1);
//Serial.print(",\t");
//Serial.print("servo_comm2");Serial.print(servo_comm2);
//Serial.println(",\t");
#endif
}
//delay(delaytime3);
//Wait 1second=1000mseconds
}
else{
servo_comm1=(int)(1500+rudder-elevator+glide_deg);
servo_comm2=(int)(1500+rudder+elevator-glide_deg);
servo_comm3=(int)(1500
+ rearwingtrim -elevator+glide_deg);
servo_comm4=(int)(1500 + rearwingtrim
+elevator-glide_deg);
servo_left.writeMicroseconds(servo_comm1); //
servo position in variable 'pos'
servo_right.writeMicroseconds(servo_comm2);
// servo position in variable 'pos'
servo_left2.writeMicroseconds(servo_comm3);
// servo position in variable 'pos'
servo_right2.writeMicroseconds(servo_comm4);
// servo position in variable 'pos'
#ifdef DOPRINTS
//Serial.print("servo_comm1");Serial.print(servo_comm1);
//Serial.print(",\t");
//Serial.print("servo_comm2");Serial.print(servo_comm2);
//Serial.println(",\t");
#endif
}
if (elapsed_time > (delaytime4 +
delaytime3 + delaytime4 + delaytime3)/1000) {// one full flap is finisheddt
elapsed_time = 0; // start next
flapping cycle
}
}
New Version Front and Rear wing move by Ailron channel --ch1
240307SFO4servoCODEforZipRearwingtrimact ZIP file
//240307 rearwingtrim act 230518 version3 added Rear wing Aileron same
move 221208 Version2 added ch7 RearWingTrim 210916 15-4th Code 4servo Rudder ElevatorUP FlapAmpUP not delaytime
control PPMRX Ch1-4 writeMicroseconds(1000-2000uS) use 5V3A FrontServoLate RearServo Ailron act only Front
servo dt and elapsed time by K.Kakuta
#include
<Servo.h>
#include
<PPMReader.h>
//#include
<InterruptHandler.h> // 2022/01/27 Delete for more good move
//#define
DOPRINTS // if you print on PC screen Data, delete”//”
//
In use of Flap system,cut "Serial.print "(put ”//”)
//and
write code to Arduino board.
int
interruptPin = 2;
int
channelAmount = 8;
PPMReader
ppm(interruptPin, channelAmount);
int
servo_right_pin = 5;
int
servo_left_pin = 6;
int
servo_right2_pin = 8;
int
servo_left2_pin = 9;
volatile
int elevator = 0;
volatile
int flapamp = 0;
volatile
int rearwingtrim =0;
//volatile
int delaytime = 100;// Servo speed low-increase Servo speed high-decrease this.
// unit: micro second
//volatile
int delaytime2 = 100;
//volatile
int delaytime3=100; // Servo speed low-increase Servo speed high-decrease this.
// unit: micro second
//volatile
int delaytime4 = 100;
volatile float
delaytime = 100;// Servo speed low-increase Servo speed high-decrease this.
// unit: micro second
volatile float
delaytime2 = 100;
volatile float
delaytime3=100; // Servo speed low-increase Servo speed high-decrease this.
// unit: micro second
volatile float
delaytime4 = 100;
float elapsed_time =
0;
float dt;
unsigned long
current_time, prev_time;
volatile
int ch3value = 1000;//Ch3
volatile
int ch1value = 1500;//Ch1
volatile
int ch2value = 1500;//Ch2
volatile
int ch4value = 1500;//Ch4
volatile
int ch5value = 1500;//Ch5
volatile
int ch6value = 1500;//Ch6 Right LeverSwitch
volatile
int ch7value = 1500;//Ch7 rear wing trim
static
int servo_comm1 = 0;// Left or Right Servo high point and low point
static
int servo_comm2 = 0; // Left or Right Servo high point and low point
static
int servo_comm3 = 0;// Left or Right Servo high point and low point no rudder
static
int servo_comm4 = 0; // Left or Right Servo high point and low point no rudder
volatile
int rudder = 0;
float
glide_deg = 0; // Gliding angle 0=0 degree 500=90degree
static
float servo_zero1 = 0;//flap angle adjust
static
float servo_zero2 = 0; //flap angle adjust
Servo
servo_left, servo_right, servo_right2, servo_left2; // create servo object to
control a servo
void
setup() {
Serial.begin(9600);
pinMode(servo_left_pin, OUTPUT);
pinMode(servo_right_pin, OUTPUT);
pinMode(servo_left2_pin, OUTPUT);
pinMode(servo_right2_pin, OUTPUT);
servo_left.attach(servo_left_pin);//output
pin No
servo_right.attach(servo_right_pin); //output
pin No
servo_left2.attach(servo_left2_pin);//output
pin No
servo_right2.attach(servo_right2_pin);
//output pin No
delay(2000);//Avoid
abnormal positions at startup-wait 2 second until RX starts220306
}
void
loop() {
prev_time = current_time;
current_time = micros();
dt = (current_time - prev_time)/1000000.0;
elapsed_time = elapsed_time + dt; // total
time spent in the main loop since beginning one upstroke/downstroke
ch3value = ppm.rawChannelValue(3);//Ch3
ch1value = ppm.rawChannelValue(1);//Ch1
ch2value = ppm.rawChannelValue(2);//Ch2
ch4value = ppm.rawChannelValue(4);//Ch4
ch5value = ppm.rawChannelValue(5);//Ch5
ch6value
= ppm.rawChannelValue(6);//Ch6
ch7value
= ppm.rawChannelValue(7);//Ch7
#ifdef DOPRINTS
Serial.print("ch3value
");Serial.print(ch3value);
Serial.print(",\t");
Serial.print("ch1value
");Serial.print(ch1value);
Serial.print(",\t");
Serial.print("ch2value
");Serial.print(ch2value);
Serial.print(",\t");
Serial.print("ch4value
");Serial.print(ch4value);
Serial.print(",\t");
Serial.print("ch5value
");Serial.print(ch5value);
Serial.println(",\t");
Serial.print("ch6value
");Serial.print(ch6value);
Serial.println(",\t");
#endif
rudder=(int)(ch1value-1500);//Ch1 Flap angle incline
elevator=(int)(ch2value-1500);//Ch2 Flap Angle
bilateral UP&Down
flapamp=(int)(ch4value-1500);//Ch4 Right and
left Flap angle difference
//delaytime=(int)((ch5value-950)/5);//Ch5
Flapping frequency
//delaytime2=(int)((ch6value-950)/10);//Ch6
Delay frontServo-RearServo
//rearwingtrim=(int)(ch7value-1500);//Ch7
Right and left angle trim difference
delaytime=( ch5value -950)/5;//Ch5 Flapping frequency
delaytime2=( ch6value -950)/10;//Ch6 Delay frontServo-RearServo
rearwingtrim =(int)( ch7value - 1500);//Ch7 Right and left angle trim difference
delaytime4 =
delaytime * delaytime2 / 200;
delaytime3 = delaytime-
delaytime4;
//
you can change UP or Down direction by your transmitter Reverse setting of each
Channel
#ifdef DOPRINTS
//Serial.print("rudder");Serial.print(rudder);
//Serial.print(",\t");
//Serial.print("elevator");Serial.print(elevator);
//Serial.print(",\t");
//Serial.print("flapamp");Serial.print(flapamp);
//Serial.print(",\t");
//Serial.print("delaytime");Serial.print(delaytime);
//Serial.print(",\t");
//Serial.print("delaytime2");Serial.print(delaytime);
//Serial.print(",\t");
#endif
if (ch3value > 1080) {
if (elapsed_time < delaytime4/1000) {
servo_comm1 = (int)( (ch3value -1000)/2+1500 + rudder - elevator+ servo_zero1+ flapamp);
servo_comm2 = (int)(1000 + (2000 - ((ch3value
-1000)/2+1500)) + rudder + elevator- servo_zero2+flapamp);
servo_comm3 = (int)( (ch3value -1000)/2+1500 + rudder - elevator + rearwingtrim + servo_zero1+ flapamp);
servo_comm4 = (int)(1000 + (2000 - ((ch3value
-1000)/2+1500)) + rudder + elevator + rearwingtrim
- servo_zero2+flapamp);
// Serial.print("servo_comm1
");Serial.print(servo_comm1);
// Serial.print(",\t");
// Serial.print("servo_comm2 ");Serial.print(servo_comm2);
// Serial.print(",\t");
// Serial.print("servo_comm3
");Serial.print(servo_comm3);
//Serial.print(",\t");
// Serial.print("servo_comm4
");Serial.print(servo_comm4);
servo_left2.writeMicroseconds(servo_comm3);
// Rear left servo position
servo_right2.writeMicroseconds(servo_comm4);
// Rear right servo position
}
//delaytime4
= delaytime * delaytime2 / 200;
//delay(delaytime4);
//Wait 1second=1000mseconds
if ((elapsed_time >
delaytime4/1000) && ( elapsed_time < (delaytime4 +
delaytime3)/1000)) {
servo_left.writeMicroseconds(servo_comm1); //
Front left servo position
servo_right.writeMicroseconds(servo_comm2);
// Front right servo position
}
//delaytime3
= delaytime- delaytime4;
//delay(delaytime3);
//Wait 1second=1000mseconds
if ((elapsed_time >
(delaytime4 + delaytime3)/1000) && (elapsed_time < (delaytime4 +
delaytime3 + delaytime4)/1000)) {
servo_comm1 = (int)( (ch3value -1000)/2+1500
+ rudder + elevator+ servo_zero1-flapamp);
servo_comm2 = (int)(1000 + (2000 - ((ch3value
-1000)/2+1500)) + rudder - elevator- servo_zero2-flapamp);
servo_comm3 = (int)( (ch3value -1000)/2+1500 + rudder + elevator+ rearwingtrim + servo_zero1-flapamp);
servo_comm4 = (int)(1000 + (2000 - ((ch3value
-1000)/2+1500)) + rudder - elevator + rearwingtrim
- servo_zero2-flapamp);
servo_left2.writeMicroseconds(servo_comm4);
// Rear left servo position
servo_right2.writeMicroseconds(servo_comm3);
// Rear right servo position
}
//delay(delaytime4);
//Wait 1second=1000mseconds
if
(elapsed_time > (delaytime4 + delaytime3 + delaytime4)/1000) {
servo_left.writeMicroseconds(servo_comm2); //
Front left servo position
servo_right.writeMicroseconds(servo_comm1);
// Front right servo position
#ifdef DOPRINTS
//Serial.print("servo_comm1");Serial.print(servo_comm1);
//Serial.print(",\t");
//Serial.print("servo_comm2");Serial.print(servo_comm2);
//Serial.println(",\t");
#endif
}
//delay(delaytime3);
//Wait 1second=1000mseconds
}
else{
servo_comm1=(int)(1500+rudder-elevator+glide_deg);
servo_comm2=(int)(1500+rudder+elevator-glide_deg);
servo_comm3=(int)(1500
+ rearwingtrim + rudder -elevator+glide_deg);
servo_comm4=(int)(1500 + rearwingtrim + rudder +elevator-glide_deg);
servo_left.writeMicroseconds(servo_comm1); //
servo position in variable 'pos'
servo_right.writeMicroseconds(servo_comm2);
// servo position in variable 'pos'
servo_left2.writeMicroseconds(servo_comm3);
// servo position in variable 'pos'
servo_right2.writeMicroseconds(servo_comm4);
// servo position in variable 'pos'
#ifdef DOPRINTS
//Serial.print("servo_comm1");Serial.print(servo_comm1);
//Serial.print(",\t");
//Serial.print("servo_comm2");Serial.print(servo_comm2);
//Serial.println(",\t");
#endif
}
if
(elapsed_time > (delaytime4 + delaytime3 + delaytime4 + delaytime3)/1000)
{// one full flap is finisheddt
elapsed_time = 0; // start next
flapping cycle
}
}
