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.
#define HYSTERIS_GAP 200.
#define HYSTERIS_VAL 100.

#define SIMULATED

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) {
  float winch_out;
  float winch_out_dt, winch_out_before, winch_out_result;
  int ele_out;
  int ail_l_out;
  int ail_r_out;

  const float phiMult = 1.0 / 1e6 * 2. * PI * F_CURVE;
  float phi        = (float)  now      * phiMult;
  float phi_before = (float) (now - 100) * phiMult;
  
  ele_out   = ele_in + AMP_MUL_ELE   * thrust_in * sin(phi);
  winch_out        = AMP_MUL_WINCH * thrust_in * sin(phi        + PHASE * PI / 180.f);
  winch_out_before = AMP_MUL_WINCH * thrust_in * sin(phi_before + PHASE * PI / 180.f);
  winch_out_dt     = winch_out - winch_out_before;
  winch_out_result = winch_out_dt < 0 && abs(winch_out) < (HYSTERIS_GAP / 2.0) ? HYSTERIS_VAL : winch_out;
//  Serial.print  (HYSTERIS_GAP / 2.0);
//  Serial.print  ('\t');
//  Serial.print  (abs(winch_out));
//  Serial.print  ('\t');
//  Serial.print  (winch_out_dt * 200.0);
//  Serial.print  ('\t');
//  Serial.print  (winch_out_result);
//  Serial.println('\t');

  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_result;
}

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

void loop() {
  #ifndef SIMULATED
  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;
  #else
  int ail_in = 500;
  unsigned long last = now; // do this afer reading first channel
  now = micros() - ail_in;
  dt = now - last;
  int ele_in = 250;
  int thrust_in = 400;
  #endif

  Serial.print  (ail_in);
  Serial.print  ('\t');
  Serial.print  (ele_in);
  Serial.print  ('\t');
  Serial.print  (thrust_in);
  Serial.print  ('\t');
  
  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);

  Serial.print  (ail_r_out);
  Serial.print  ('\t');
  Serial.print  (ail_l_out);
  Serial.print  ('\t');
  Serial.print  (ele_out);
  Serial.print  ('\t');
  Serial.print  (winch_out);
  Serial.println  ('\t');

//  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);
}