Arduino sketch for controlling ornitoptera using KST-320 servos

sketch_jun03_raven.ino

//Arduino servo flap system
//copyright Steve Morris 10-25-16

#include <DigitalServo.h>

#include <PPMReader.h>
#include <InterruptHandler.h>

int interruptPin = 2;
int channelAmount = 6;
PPMReader ppm(interruptPin, channelAmount);

int servo_left_pin = 4;
int servo_right_pin = 6;


float elevator = 0;
volatile int ppm1_value = 1000;
volatile int ppm2_value = 1550;
volatile int ppm3_value = 1500;
volatile int ppm5_value = 1500;
volatile int prev_timeD2 = 0;
volatile int prev_timeD3 = 0;
static int servo_comm1 = 0;
static int servo_comm2 = 0;
volatile int flapangle1 = 0;
volatile int flapangle2 = 0;
volatile int rudder = 0;
float rudderamp = 0;
volatile int millisold = 0;
int millinow = 0;
float dt = 0;
float flapmag = 0;
static float flapdeg = 0;
float tcommand = 0;
float floattime = 0;
float glide_deg = 6.0;
float omegadot = 0.0;
float thetadot = 0.0;
static float omega = 0.0;
static float theta = 0.0;
static float k0 = 1.0;
static float k2 = 10.0;
static float servo_zero1 = 0;
static float servo_zero2 = 0;

DigitalServo servo_left, servo_right; // create servo object to control a servo


void setup() {
  // Serial.begin(9600);

  pinMode(servo_left_pin, OUTPUT);
  pinMode(servo_right_pin, OUTPUT);

  servo_left.attach(servo_left_pin);
  servo_right.attach(servo_right_pin);
}


void loop() {
  ppm1_value = ppm.rawChannelValue(3);
  ppm2_value = ppm.rawChannelValue(1);
  ppm3_value = ppm.rawChannelValue(2);
  ppm5_value = ppm.rawChannelValue(5);

	millinow = millis();
	floattime = millinow / 1000.0;
	dt = (millinow - millisold) / 1000.0;
	millisold = millinow;

	tcommand = (ppm1_value - 480.0) / 8.22;
	omegadot = k0*tcommand - k2*omega;
	thetadot = omega;
	flapdeg = sin(theta);
	theta = theta + omega*dt;
	omega = omega + omegadot*dt;
  // ppm1_value = 2000
  // (ppm1_value - 1000) / 25.0 + 6
  flapmag = (ppm1_value - 1055) / 45.0 + 2;
	flapdeg = flapmag * sin(theta); //variable amplitude+freq

  rudderamp = ((1548.0 / ppm2_value) - 1) / 8 + 1;
	rudder = (int)((ppm2_value - 1540) / 50);
  elevator = (ppm3_value / 33 - 45) * (ppm5_value / 700);

	flapangle1 = (int)((rudder - (flapdeg / rudderamp) + servo_zero1 - elevator) * 2.0);
	flapangle2 = (int)((rudder + (flapdeg * rudderamp) + servo_zero2 + elevator) * 2.0);

	if (ppm1_value > 1040) {
  	servo_comm1 = flapangle1;
  	servo_comm2 = flapangle2;
	}

	// Glide Lock
	if (ppm1_value < 1030) {
		servo_comm1 = (int)((rudder - glide_deg + servo_zero1 - elevator) * 2.0);
		servo_comm2 = (int)((rudder + glide_deg + servo_zero2 + elevator) * 2.0);
	}

  // Serial.println(ppm1_value);
  // Serial.print(servo_comm1);
  // Serial.print(",  ");
  // Serial.print(servo_comm2);
  // Serial.print(",  ");

	servo_comm1 = servo_comm1 + 100;
	servo_comm2 = servo_comm2 + 100;

	servo_left.write(servo_comm1); // tell servo to go to position in variable 'pos'
  servo_right.write(servo_comm2); // tell servo to go to position in variable 'pos'
  //
  // Serial.println(ppm1_value);
  // Serial.print(",  ");
  // Serial.print(ppm2_value);
  // Serial.print(",  ");
  // // Serial.print(1548.0 / ppm2_value);
  // // Serial.print(",  ");
  // Serial.print(rudderamp);

  // Serial.println(rudderamp);

  // Serial.print(",  ");
  // Serial.print(elevator);
  // Serial.print(",  ");
  // Serial.print(servo_comm1);
  // Serial.print(",  ");
  // Serial.print(servo_comm2);
  // // Serial.println();
  //
  // Serial.print(flapdeg);
  // Serial.print(",  ");
  // Serial.print(floattime);
  // Serial.print(",  ");
  // Serial.print(dt);
  //
  // Serial.print(",  ");
  // Serial.print(flapangle2);

  // Serial.print(",  ");
  // Serial.println(");
  // Serial.print(tcommand);

  // Serial.print(",  ");
  // Serial.println(ppm1_value);
}