Сервопривод дрожит около целевого положения при любом ненулевом интегральном коэффициенте PID

[Yuriy-Shved]

Приветствую.
Примененная библиотека GyverPID дает эффект дрожания около целевой точки.

#include <DueTimer.h>
#define PID_INTEGER
// #define PID_INTEGRAL_WINDOW 50
// #define PID_OPTIMIZED_I
#include <GyverPID.h>
double Kp_L = 0.0;
double Ki_L = 0.3;
double Kd_L = 0.0;
double Kp_R = 0.0;
double Ki_R = 0.3;
double Kd_R = 0.0;
int16_t SET_L = 511;
int16_t SET_R = 511;
#define PWM_R 8
#define DIR_R 10
#define A_R A2
#define PWM_L 9
#define DIR_L 11
#define A_L A3
uint32_t timing = 0;
uint32_t timing_dt = 0;
uint32_t dt_delay = 100;
uint32_t print_delay = 1000;
uint32_t slow = 0;
int16_t cur_pos_L;
int16_t cur_pos_R;
int16_t RC_L;
int16_t RC_R;
//
int16_t toLow_pos = 9;
int16_t toHigh_pos = 1023 - toLow_pos;
int16_t STOP_Low_pos = 5;
int16_t STOP_High_pos = 1023 - STOP_Low_pos;
bool trig = false;

bool dirL;
bool dirR;
int32_t timing_dirL = 0;
int32_t timing_dirR = 0;
int16_t GyverPID_L;
int16_t GyverPID_R;
GyverPID regulator_L(Kp_L, Ki_L, Kd_L, dt_delay);
GyverPID regulator_R(Kp_R, Ki_R, Kd_R, dt_delay);

void setup() {
  Serial.begin(115200);
  pinMode(DIR_R, OUTPUT);
  digitalWrite(DIR_R, LOW);
  pinMode(DIR_L, OUTPUT);
  digitalWrite(DIR_L, LOW);
  pinMode(PWM_R, OUTPUT);
  pinMode(PWM_L, OUTPUT);

  analogReadResolution(10);
  analogWriteResolution(8);

  regulator_L.setLimits(-255, 255);   
  regulator_R.setLimits(-255, 255);   
  regulator_L.setpoint = SET_L;
  regulator_R.setpoint = SET_R;
}

void loop() {

if (millis() - timing_dt >= dt_delay){

cur_pos_L = analogRead(A_L);
cur_pos_R = analogRead(A_R);

if (SET_L - cur_pos_L < 0) {
  digitalWrite(DIR_L, HIGH);
  dirL = HIGH;
  }
  else {
  digitalWrite(DIR_L, LOW);
  dirL = LOW;
  }
  if (SET_R - cur_pos_R < 0) {
  digitalWrite(DIR_R, HIGH);
  dirR = HIGH;
  }
  else {
  digitalWrite(DIR_R, LOW);
  dirR = LOW;
  }

  if (cur_pos_L > STOP_Low_pos && cur_pos_L < STOP_High_pos) {
    regulator_L.setpoint = SET_L;
    regulator_L.input = cur_pos_L;
    GyverPID_L = round(regulator_L.getResult());
    GyverPID_L = abs(GyverPID_L);
    analogWrite(PWM_L, GyverPID_L);     
  }
  else {
    analogWrite(PWM_L, 0);
    }
  if (cur_pos_R > STOP_Low_pos && cur_pos_R < STOP_High_pos) {
  regulator_R.setpoint = SET_R;
  regulator_R.input = cur_pos_R;
  GyverPID_R = round(regulator_R.getResult());
  GyverPID_R = abs(GyverPID_R);
  analogWrite(PWM_R, GyverPID_R);
  }
  else {
    analogWrite(PWM_R, 0);
    }

  timing_dt = millis();
  }
}

Привод дрожит при указании любого ненулевого интегрального коэффициента, тем сильнее, чем больше коэффициент. То есть он постоянно перескакивает целевую точку, то в одну сторону, то в другую.
Гипотетически можно задать ему мертвую зону, где ошибка меньше заданной приравнивается к нулю. Но тогда пострадает точность.
Посоветуете, как лучше этот эффект устранить?

 

[Bruzzer]

@Yuriy-Shved,
Вашими серво точно можно управлять при помощи analogWrite ?

Наверно стоит посмотреть шумы на analogRead, если они значительны, то или усреднять, или аппаратно пытаться уменьшить.

 

[Yuriy-Shved]

@Bruzzer, резисторы действительно шумят. Для борьбы с шумом используется альфа-фильтр:

int16_t getAngle(int PIN){ // функция получения угла с потенциометра/энкодера
  static int16_t oldAngle = 0;
  int16_t angle = analogRead(PIN);
  angle = lpFilter(angle, oldAngle, alpha);   // фильтруем показания с потенциометра, если надо
  oldAngle = angle;
  return angle;
}

int16_t lpFilter(int16_t value, int16_t oldValue, float alp){
  if (alp <= 1) {
  return round(float(oldValue)*(1.f-alp)+ alp*float(value));
  }
  else {
  return value;
  }
}

Помогает, но не радикально. Основная проблема в моменте инерции и люфтах механизма. С ростом мощности момент инерции растет, люфты полностью устранить невозможно. Более того, старт движения из-за внутреннего трения редуктора происходит при немаленьком ШИМ, а сопротивление в движении несколько меньше сопротивления при страгивании.
Пока в голову ничего не приходит, кроме как прекращения суммирования в пределах зоны нечувствительности, и среза интеграла при пересечении целевой позиции.

 

[Bruzzer]

@Yuriy-Shved,
Можете сказать, какой у вас сервопривод ? (вопрос скорее из любопытства).