Проблема со счетчиком энкодеров на руле сегвея.

Я пытаюсь реализовать рулевой механизм для проекта сегвея. Идея состоит в том, чтобы определять наклон руля сегвея с помощью инкрементного поворотного энкодера (omron e6j-cwz 600ppr). Таким образом, когда я поворачиваю планку влево или вправо, сегвей тоже поворачивается. Я использую Teensy 3.2 с IDE Arduino.

Для управления двигателями я использую ШИМ. Пределы импульсов — это калиброванные значения для входного напряжения +/- 100 % (+/- 24 В). Диапазон импульсов составляет от 1,0 до 2,0 мс, при этом 1,5 мс является нулевым значением.

Вот изображение поворотного механизма.

Проблема в том, что движение рукоятки не очень плавное, и иногда после наклона рукоятки и возврата ее в прямое положение счетчик никогда не возвращается к 0, но считается некоторое значение от -6 до 6. а иногда даже от -15 до 15. Таким образом, сегвей пытается повернуть, хотя планка прямая.

Код, который я использую для определения наклона и настройки двигателей:

//Кодер поворота
#define c_TurnEncoderPinA 15
#define c_TurnEncoderPinB 14
#define TurnEncoderIsReversed
volatile bool _TurnEncoderBSet;
volatile int _TurnEncoderTicks = 0;
/** Turn position */
int TurnAngular = 0;

/** Proportional gain */
int kp = 55;

/** voltage to the motor*/
float vol = 0;
float aux;
/** calulated pwm pulse*/
float uy = 0;

/**Left motor control*/
int motorL=9;
/**Right motor control*/
int motorR=10;

//Сопоставление напряжения с ШИМ (контроллер Mc 160)
float  p1 = -25.44, p2 = 0.1894, p3 = 146.6, p4 = -0.2874, p5 =  -480, p6 =  1500; // ШИМ3

//Дискретные значения времени
const int samplingTime = 1000*10; // 10000 микросекунд = 10 мс
unsigned long previousTime, currentTime, timeDifference;
int estimationTimeCount = 1;

void setup() {
  delay(3000);
  Serial.begin(115200);
  delay(2000);
  previousTime = micros();
  // Энкодер поворота
  pinMode(c_TurnEncoderPinA, INPUT_PULLUP);      // устанавливаем контакт A как вход
  pinMode(c_TurnEncoderPinB, INPUT_PULLUP);      // устанавливаем контакт B как вход
  attachInterrupt(digitalPinToInterrupt(c_TurnEncoderPinA), HandleTurnMotorInterruptA, RISING);

  /*Initiate Output*/
  analogWriteResolution(11);
  pinMode(motorR,OUTPUT);
  pinMode(motorL,OUTPUT);
  analogWrite(motorR, 1500);
  analogWrite(motorL, 1500);

Serial.println("Start...");
}

void loop() {
  currentTime = micros();
  timeDifference = currentTime - previousTime;
  if (timeDifference > samplingTime)
  {
    previousTime = currentTime;
    sampleEncoders();
    setMotors();
    printData();
    estimationTimeCount++; 
  }
}

void sampleEncoders() {
  TurnAngular = _TurnEncoderTicks; //обновляем показания кодировщика каждые 10 мс.
}

float turn(){
  if(TurnAngular<=6 && TurnAngular>=-6)
  {
    return 0;
  }
  else 
  {
   //напряжение рассчитывается с пропорциональным коэффициентом усиления
   //умножить на угол энкодера в радианах.
    return (kp*TurnAngular*(2*pi/(pprT))); 
  }
}

void setMotors() {
    vol = turn();

    // эти две строки отображают рассчитанное напряжение на ШИМ
    // в диапазоне от 1000 до 2000.
    aux = (vol+0.01143)/13.13; 
    uy = (int) p1*pow(aux,5) + p2*pow(aux,4) + p3*pow(aux,3) + p4*pow(aux,2) + p5*(aux) + p6;
    uy = constrain(uy, 1000,  2000);

    // это просто идея, которую я должен был решить, чтобы решить эту проблему.
    // если управление в пределах +/-50 импульсов от 1500
    // устанавливаем импульс на 1500, а последнее сохраненное значение счетчика на 0.
    // каждые 2,5 секунды, если условие выполнено, то также сбрасываем фактическое значение
    // счетчик кодера
    if (uy < 1550 && uy > 1450)
    {
      uy = 1500;
      TurnAngular = 0;
      if ( estimationTimeCount%250 == 0 && _TurnEncoderTicks != 0)
      {
        _TurnEncoderTicks = 0;
      }
    }
  }

  /** 
 Left wheel (L): forward from 1500 to 1000, backward from 1500 to 2000.
 Right wheel (R): forward from 1500 to 2000, backward from 1500 to 1000.
  **/  
 //устанавливаем u для каждого колеса, чтобы сегвей поворачивал.
  analogWrite(motorR,uy);
  analogWrite(motorL,uy);
}

void printOutput()
{
  Serial.print("tout(");
  Serial.print(estimationTimeCount);
  Serial.print(", :) = [");
  Serial.print(TurnAngular);
  Serial.print(", ");
  Serial.print(_TurnEncoderTicks);
  Serial.print(", ");
  Serial.print(currentTime*pow(10,-6));
  Serial.println("];");
}

void HandleTurnMotorInterruptA()
{
  // Тестовый переход; поскольку прерывание срабатывает только при «нарастании», нам не нужно читать вывод A.
  _TurnEncoderBSet = digitalRead(c_TurnEncoderPinB);   // читаем входной контакт
  // и корректируем счетчик +, если A опережает B
  #ifdef TurnEncoderIsReversed
    _TurnEncoderTicks -= _TurnEncoderBSet ? -1 : +1;
  #else
    _TurnEncoderTicks += _TurnEncoderBSet ? -1 : +1;
  #endif
}

Исходный код счетчика кодировщиков можно найти здесь.

В код я добавил свой собственный, но иногда он дает сбой, я устанавливаю на двигатели нулевое напряжение, если счет находится в пределах +/- 6 отсчетов. Второстепенная проблема заключается в том, что когда наклон шкалы, например, составляет +7, контроль становится слишком агрессивным, поэтому я продолжаю пытаться найти идеальное пропорциональное усиление.

Наконец, я не добавил код для балансировки сегвея, главным образом потому, что это не проблема, а усилие управления просто установлено на 0, пока я пытаюсь решить проблему. Кроме того, это было бы слишком долго.

Какое решение вы могли бы придумать?

Я отредактировал функцию поворота следующим образом:

float turn(){
int8_t TurnVal = 0;
  //размермертвой зоны = 6
  if(TurnAngular <= deadZoneSize && TurnAngular >= -deadZoneSize)
  {
    return 0;
  }
  else 
  {
    if (TurnAngular<0) 
    {
      TurnVal = TurnAngular + deadZoneSize;
    } 
    else 
    {
      TurnVal = TurnAngular - deadZoneSize;
    }
    Serial.println(TurnVal);
    return (kp*TurnVal*(2*pi/(float)(pprT)));
  }

Я не могу найти способ исправить ошибку между фактическим прямым положением и измеренным прямым положением (0 отсчетов). Иногда после некоторого перемещения планки фактическое прямое положение оказывается за пределами мертвой зоны, и сигвей все равно хочет повернуть.

Вот иллюстрация.