4サーボ駆動型羽ばたき機のArduino CODE(4 Servo)
  Arduino CODE for SFODragonfly90-4S
    New CODE
2023/05/19 2024/03/08
                

                                                                                 

4 Servos Flap System for SFODragpnfly90-4S

 PPM Receiver-- RX PPM signal input to D2 pin

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

https://jp.banggood.com/5pcs-Mini-DC-DC-Adjustable-Step-Down-Power-Supply-Module-12V-24V-to-5V-3_3V-9V-3A-mini360-p-1542720.html?rmmds=myorder&cur_warehouse=CN

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

 https://usa.banggood.com/BLUEARROW-AF-D43S-6_0-MG-Micro-Metal-Gear-Digital-Servo-For-XK-K130-RC-Helicopter-p-1509395.html?rmmds=myorder&cur_warehouse=CN

RX: OrangeRx R616XN DSM2 / DSMX6CH CPPM

https://hobbyking.com/jp_jp/orangerx-r616xn-dsm2-dsmx-compatible-6ch-cppm-nano-receiver-with-failsafe.html

TX: Jumper T8SG

 

 

 



 
5V to Receiver


 6V to Receiver


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>//  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 = 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

dtelapsed 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の電圧が落ちてくると、羽ばたきの周波数が多くなるため、飛行を中止して設定しなおすか、飛行中に再設定する必要がある。


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

}

 

}