run2.ino

#define HZ 60.

#define ELE_IN 12
#define ELE_OUT 11
#define WINCH_OUT 10
#define THRUST_IN 9
#define AIL_IN 8
#define AIL_L_OUT 7
#define AIL_R_OUT 6

#define F_CURVE 0.7
#define AMP_MUL_WINCH 1.
#define AMP_MUL_ELE 1.
#define PHASE -20.

unsigned long now = 0;
unsigned long dt;

void setup() {
  pinMode(THRUST_IN, INPUT);
  pinMode(ELE_IN, INPUT);
  pinMode(ELE_OUT, OUTPUT);
  pinMode(WINCH_OUT, OUTPUT);
  pinMode(LED_BUILTIN, OUTPUT);
  analogWrite(11, 50);
  Serial.begin(115200);
}

void pulseOut(int pin, int pol, int us) {
  digitalWrite(pin, pol);
  delayMicroseconds(us);
  digitalWrite(pin, !pol);
}


int calc(int ail_in, int ele_in, int thrust_in, int *pail_l_out, int *pail_r_out, int *pele_out) {
  int winch_out;
  int ele_out;
  int ail_l_out;
  int ail_r_out;

  float phi = (float)now / 1e6 * 2. * PI * F_CURVE;
  ele_out = ele_in + AMP_MUL_ELE * thrust_in * sin(phi);
  winch_out = AMP_MUL_WINCH * thrust_in * sin(phi + PHASE * PI / 180.f);

  ail_l_out = ail_in;
  ail_r_out = -ail_in;

  *pele_out = ele_out;
  *pail_l_out = ail_l_out;
  *pail_r_out = ail_r_out;
  return winch_out;
}

void limitoutput(int pin, int value) {
  value += 1500;
  if (value<0)
    value = 0;
  if (value>3000)
    value = 3000;
  pulseOut(pin, 1, value);
}

void loop() {
  int ail_in = pulseIn(AIL_IN, 1)-1500;
  unsigned long last = now; // do this afer reading first channel
  now = micros() - ail_in;
  dt = now - last;
  int ele_in = pulseIn(ELE_IN, 1)-1500;
  int thrust_in = pulseIn(THRUST_IN, 1)-1500;

  int ail_l_out;
  int ail_r_out;
  int ele_out;
  int winch_out = calc(ail_in, ele_in, thrust_in, &ail_l_out, &ail_r_out, &ele_out);

//  unsigned long t2 = micros();
//  unsigned long dt = t2 - now;
//  delayMicroseconds(4000-dt);

  limitoutput(WINCH_OUT, winch_out);
  limitoutput(ELE_OUT, ele_out);
  limitoutput(AIL_L_OUT, ail_l_out);
  limitoutput(AIL_R_OUT, ail_r_out);
}