Arduino.ru

Помогите пожалуйста отфильтровать показания энкодера.

Суть проблемы в том, что сделал ПИД регулятор для колеса машинки, но из-за большого шума колесо на малых оборотах сильно дергается.

вот такие скачки у энкодера при равномерном вращении колеса

и собственно код программы

#include <PID_v1.h>
#include <Wire.h>
#define encoderPin 2
byte    acc = 0;
byte    ADDRESS = 0;
volatile double  numberOfImpulses = 0;

double  Setpoint, Input, Output, X;
PID     myPID(&Input, &Output, &Setpoint, 20.75, 50.4, 0.09935, DIRECT);
int     IN3 = 5; // Input3 подключен к выводу 5
int     IN4 = 4; // Input4 подключен к выводу 4
int     ENB = 11; // ENB    подключен к выводу 3 (PWM)
union   ReceiveSpeed{ //Принимаемая скорость от ведущего
  short SpeedSh;
  char  SpeedCh[2];
}Speed;
void  interrupt0(){   // Функция прерывания на втором пине (D2)
  numberOfImpulses++;
  return;
}

void  setup(){        // Функция инициализации 
Serial.begin (57600);
   //ENCODER
  pinMode (encoderPin, INPUT);
  attachInterrupt(0, interrupt0, RISING); //Encoder pin on interrupt 0 (pin D2)
  //MOTORSHIELD
  pinMode (ENB, OUTPUT);
  pinMode (IN3, OUTPUT);
  pinMode (IN4, OUTPUT);
  Setpoint=17; // заданная скорость
  
  myPID.SetSampleTime(100); //dt для ПИД-регулятора
  myPID.SetOutputLimits(0, 255); //допустимые границы управления ПИД-регулятора
  myPID.SetMode(AUTOMATIC); //включаем ПИД-регулятор
  interrupts();  
}
void  loop(){         // Основной цикл программы
  
  Input = numberOfImpulses;
  if (myPID.Compute()){
    analogWrite(ENB, Output);
    digitalWrite (IN3, HIGH);
    digitalWrite (IN4, LOW);
   
 /*   Serial.print("numberOfImpulses=");
    Serial.print(numberOfImpulses);
    Serial.print(" ");
    Serial.print("Setpoint=");
    Serial.print(Setpoint);
    Serial.print(" ");
    Serial.print("Output=");
    Serial.print(Output);
    Serial.print(" ");
    Serial.print("Input=");
    Serial.println(Input);
*/ для наблюдения за показаниями через монитор порта

    numberOfImpulses = 0;
   
  }
}