Arduino.ru

Всем привет!!!

Вот пытаюсь подружить Arduino due с пультом от самалетика (модель пульта FS-CT6B), что-бы управлять платформой с двумя двигателями...

Пульт 6-канальный, на приемнике выдает импульсы с частотой от 1500 до 2000мкс....

Что-бы принимать эти сигналы использовал прерывание...

Если посмотреть в терминале через serial соединение она прекрасно работает, а если подключить к платформе то начинает тупить(крутиться, вертиться, идет прямо взяд сама по себе)

volatile unsigned long Time1=0;
volatile unsigned long Time2=0;
volatile unsigned long Time3=0;
volatile unsigned long Time4=0;
volatile unsigned long Time5=0;
long maxi1 = 1877;
long sredni1 = 1537;
long mini1 = 1185;
long value1 = 0;
long maxi2 = 1877;
long sredni2 = 1537;
long mini2 = 1185;
long value2 = 0;
long resultat1 = 0;
long resultat2 = 0;
boolean flag1=1;
boolean flag2=1;
boolean flag3=1;
boolean flag4=1;

int motor_speed1 = 0;
int motor_speed2 = 0;
int Motor_speed_pin1 = 7;
int Motor_speed_pin2 = 8;
int Motor_pin1=26;
int Motor_pin2=28;
int Motor_pin3=30;
int Motor_pin4=32;
int sL;
int sR;



void setup() {
  
  attachInterrupt(22, Start, CHANGE);
  attachInterrupt(24, Vector, CHANGE);
  pinMode(Motor_speed_pin1,OUTPUT);
  pinMode(Motor_speed_pin2,OUTPUT);
  pinMode(Motor_pin1,OUTPUT);
  pinMode(Motor_pin2,OUTPUT);
  pinMode(Motor_pin3,OUTPUT);
  pinMode(Motor_pin4,OUTPUT);
  digitalWrite(Motor_pin1,0);
  digitalWrite(Motor_pin2,0);
  digitalWrite(Motor_pin3,0);
  digitalWrite(Motor_pin4,0);
  analogWrite(Motor_speed_pin1,0);
  analogWrite(Motor_speed_pin2,0);
  Time5=millis();
  
}

void spede1(long value1){  //декодирование сигнали из пульты, ответ выводить в resultat1 в диапазон от0 до 255, если flag3=1 то это значить что мы идем вперед

if (value1 != 0) {
    value1 = constrain(value1,mini1,maxi1);
    if (value1 >= sredni1) {
      value1 = map(value1,sredni1,maxi1,0,255);
      flag3=1;}
      else {
        value1 = map(value1,sredni1,mini1,0,255);
        flag3=0;}
        if (value1>70)
        resultat1 = value1;
        else resultat1=0;
    
  
  
}
}
void spede2(long value2){ //декодирование сигнали из пульты, ответ выводить в resultat2 в диапазон от0 до 255, если flag4=1 то это значить что мы повернули направо
if (value2 != 0) {
    value2 = constrain(value2,mini2,maxi2);
    if (value2 >= sredni2) {
      value2 = map(value2,sredni2,maxi2,0,255);
      flag4=1;}
      else {
        value2 = map(value2,sredni2,mini2,0,255);
        flag4=0;}
       if (value2>70)
        resultat2 = value2;
        else resultat2=0;
    
}
}

void Vector(){ //получение сигнала из пульта сигнала
if (flag2==1) {
    Time3=micros();
    flag2=0;}
    else {
      Time4=micros();
      value2=Time4-Time3;
      flag2 = 1;
      }
    }


void Start(){ //получение сигнала из пульта сигнала
  
  if (flag1==1) {
    Time1=micros();
    flag1=0;}
    else {
      Time2=micros();
      value1=Time2-Time1;
      flag1 = 1;
      }
      
}
void Lev_motor(int sL){  //функция указание направления движение мотора 
if (sL==1){
  digitalWrite(Motor_pin1,1);
  digitalWrite(Motor_pin2,0);

  } else{
    if (sL==2){
      digitalWrite(Motor_pin1,0);
      digitalWrite(Motor_pin2,1);

    } else{
      if (sL==0){
        digitalWrite(Motor_pin1,0);
        digitalWrite(Motor_pin2,0);

      }
    }
  }

}
void Right_motor(int sR){   //функция указание направления движение мотора
if (sR==1){
  digitalWrite(Motor_pin3,1);
  digitalWrite(Motor_pin4,0);

  } else{
    if (sR==2){
  digitalWrite(Motor_pin3,0);
  digitalWrite(Motor_pin4,1);

    } else{
      if (sR==0){
        digitalWrite(Motor_pin3,0);
        digitalWrite(Motor_pin4,0);
       
      }
    }
  }
  
  }




void loop() {
  spede1(value1);
  spede2(value2);
   
  if (resultat1==0){
    if (resultat2==0){
      Lev_motor(0);
      Right_motor(0);
    }
    else{
      if (flag4==1){
        Lev_motor(1);
        Right_motor(2);
      }
      else{
        Lev_motor(2);
        Right_motor(1);
      }
    }
  }
  else {
    if (resultat2==0){
      if (flag3==1){
        Lev_motor(1);
        Right_motor(1);
      }
      else{
        Lev_motor(2);
        Right_motor(2);
      }
    }
    else{
      if (flag3==1) {
        if (flag4==1){
          Lev_motor(1);
          Right_motor(0);          
        }
        else{
          Lev_motor(0);
          Right_motor(1);
        }
      }
      else{
        if (flag4==1){
          Lev_motor(0);
          Right_motor(2);
        }
        else{
          Lev_motor(2);
          Right_motor(0);
        }
      }
    }
  }

  

   
}