Polskie Forum FPV

Kod: Zaznacz cały

#include <PinChangeInterrupt.h>
#include <Servo.h> 
const byte channel_pin[] = {3, 4, 5};
volatile unsigned long rising_start[] = {0, 0, 0};
volatile long channel_length[] = {0, 0, 0};

Servo servo1; 
Servo servo2; 
int gain1 = 2;
int gain2 = 2;
int s1= 0;
int s2= 0;
int s11= 0;
int s12= 0;

void setup() {
  Serial.begin(57600);
  servo1.attach(9); // Servo 1 - pin 9
  servo2.attach(10); // Servo 2 - pin 10
  pinMode(channel_pin[0], INPUT);
  pinMode(channel_pin[1], INPUT);
  
  attachPinChangeInterrupt(digitalPinToPinChangeInterrupt(channel_pin[0]), onRising0, CHANGE);
  attachPinChangeInterrupt(digitalPinToPinChangeInterrupt(channel_pin[1]), onRising1, CHANGE);
}

void processPin(byte pin) {
  uint8_t trigger = getPinChangeInterruptTrigger(digitalPinToPCINT(channel_pin[pin]));

  if(trigger == RISING) {
    rising_start[pin] = micros();
  } else if(trigger == FALLING) {
    channel_length[pin] = micros() - rising_start[pin];
  }
}
void onRising0(void) {
  processPin(0);
}
void onRising1(void) {
  processPin(1);
}

void loop() {

s11 = (channel_length[1]+channel_length[0])/gain1;
s12 = (channel_length[1]-channel_length[0])/gain2;

 if (s11 > 1500)  // Doregulowanie strony silnikow
{
  s11 = (s11*1.1);  // Mnoznik
}
 if (s12 < 1500)   // Dregulowanie strony silnikow
{
  s12 = (s12*0.9);  // Mnoznik
}
servo1.writeMicroseconds(s11);
servo2.writeMicroseconds(s12);

  Serial.print(s11);
  Serial.print(" | ");
  Serial.print(s12);
  Serial.print(" | ");
  Serial.print(channel_length[0]);
  Serial.print(" | ");
  Serial.println(channel_length[1]);
}