Как откалибровать ФИД?

Я работаю над устройством левитирующего магнита с ПИД-управлением. Он использует датчик Холла для включения и выключения электромагнита, чтобы достичь левитации.

Когда я подношу магнит близко к датчику Холла, я чувствую пульсацию от электромагнита, однако, если я отпускаю магнит, он каждый раз всегда прилипает к электромагниту, как будто PID не откалиброван должным образом, код, который я использую, поставляется с предварительно встроенной возможностью калибровки ПИД-регулятора, вы можете изменить уставку, пропорцию, интеграл и производную. Я пытался с этим возиться, но, похоже, это не имеет никакого значения. .

/*
Магическая левитация

1) подключите цифровой контакт PIN_COIL к затвору MOSFET для активации электромагнита.
2) соедините контакт аналогового входа PIN_HALL_SENSOR с сигнальным контактом датчика Холла (другой
два контакта должны быть подключены к земле и +5В).
3) (необязательно) подключите цифровой контакт PIN_CTRL_INDICATOR к светодиоду, который указывает, когда
цикл контроллера работает.
4) запускаем Arduino, на последовательном порту доступны следующие команды:
- нажмите «s» или «S», чтобы уменьшить или увеличить заданное значение,
- нажмите «p» или «P», чтобы уменьшить или увеличить усиление P,
- нажмите «d» или «D», чтобы уменьшить или увеличить усиление D,
- нажмите «i» или «I», чтобы уменьшить или увеличить усиление I,
- нажмите «x» или «X», чтобы показать текущие настройки
5) система переходит в режим IDLE с минимальной ШИМ-командой, позволяющей сориентировать
постоянным магнитом, в пределах досягаемости система переходит в режим УПРАВЛЕНИЯ. Когда магнит находится слишком близко к
катушки, система переключается в режим ВЫКЛ, отключая ШИМ-команду.

версия 1.0: 23.12.2011 - Э. Ассенза
*/

const int PIN_COIL = 11; // Контакты 3 и 11 подключены к Таймеру 2.
const int PIN_HALL_SENSOR = 0;
const int PIN_CTRL_INDICATOR = 13;
// Сопротивление катушки 3 Ом, макс. 1 А = макс. 3 Вольт.
const int COIL_MAX_PWM_VALUE = 160;
// Минимальное магнитное поле в режиме ожидания (необходимо для правильной ориентации магнита).
const int COIL_IDLE_VALUE = 50;
// Пороги приближения, когда магнитное поле в этом диапазоне входит в режим управления.
const int MAG_PROX_THRESHOLD_MIN = 585;
const int MAG_PROX_THRESHOLD_MAX = 615;
// Пределы магнитного поля, когда вне этого диапазона входит в режим ожидания.
const int MAG_LIMIT_MIN = 560;
const int MAG_LIMIT_MAX = 700;
// Частота контура управления.
const int CTRL_LOOP_PERIOD_MS = 1;
// Значение смещения контура управления (средняя точка ШИМ)
const int CONTROL_BIAS = 65;
// Уставка, соответствующая смещению управления
const int CONTROL_SETPOINT = 597;
const int MAX_ERR_INTEGRAL = 10000;
// параметры ПИД-регулятора
const float PGain = 2.1;
const float DGain = 23.6;
const float IGain = 0.001;

const float P_INCR = 0.1;
const float D_INCR = 0.1;
const float I_INCR = 0.001;

// Константы конечного автомата.
const byte MODE_OFF = 0;
const byte MODE_IDLE = 1;
const byte MODE_CONTROL = 2;

const char     *gAppName = "Arduino Magnetic Levitation";
byte            gMode;
int             gSetPoint;
int             gSensorReadout;
int             gPrevSensorReadout;
int             gPwmCommand;
int             gIntegral;
float           gBias;
float           gP;
float           gD;
float           gI;
unsigned long   gMillisCounter;

void setupCoilPWM()
{
   // Настройте таймер 2 как фазово-корректирующий ШИМ, 3921 Гц.
   pinMode(3, OUTPUT);
   pinMode(11, OUTPUT);
   // Регистр таймера 2: WGM20 устанавливает правильный режим фазы ШИМ, COM2x1 устанавливает выход ШИМ на каналы A и B.
   TCCR2A = 0;
   TCCR2A = _BV(COM2A1) | _BV(COM2B1) | _BV(WGM20);
   // Установите предварительный делитель на 8, частота ШИМ равна 16 МГц/255/2/<предделитель>
   TCCR2B = 0;
   TCCR2B = _BV(CS21);
}

void writeCoilPWM(uint8_t pin, int val)
{
   if( pin == 3 )
   {
      OCR2B = val;
   }
   else if( pin == 11 )
   {
      OCR2A = val;
   }
   else
   {
      OCR2A = 0;
      OCR2B = 0;
   }
}

void processCommand( int cmd )
{
   char tmp[64];

   switch(cmd)
   {
      case 'p':
         gP -= P_INCR;
         if( gP < 0 ) gP = 0;
         break;
      case 'P':
         gP += P_INCR;
         break;
      case 'd':
         gD -= D_INCR;
         if( gD < 0 ) gD = 0;
         break;
      case 'D':
         gD += D_INCR;
         break;
      case 'i':
         gI -= I_INCR;
         if( gI < 0 ) gI = 0;
         break;
      case 'I':
         gI += I_INCR;
         break;
      case 's':
         gSetPoint --;
         break;
      case 'S':
         gSetPoint ++;
         break;
      case 'x':
      case 'X':
         sprintf(tmp, "s(%d) c(%d) h(%d) P(%u) D(%u) I(%u,%d)\n", gSetPoint, gPwmCommand, gSensorReadout, (uint16_t)(gP*1000.0), (uint16_t)(gD*1000.0), (uint16_t)(gI*1000.0), gIntegral );
         Serial.print(tmp);
         break;
      default:
         break;
   }
}

byte executeOffMode()
{
   digitalWrite(PIN_CTRL_INDICATOR, LOW);

   // выключаем магнитное поле
   gPwmCommand = 0;
   writeCoilPWM(PIN_COIL, gPwmCommand);

   gSensorReadout = analogRead(PIN_HALL_SENSOR);

   if( gSensorReadout <= MAG_LIMIT_MIN )
   {
      // Переключиться в режим ожидания.
      return MODE_IDLE;
   }
   else
   {
      return MODE_OFF;
   }
}

byte executeIdleMode()
{
   digitalWrite(PIN_CTRL_INDICATOR, LOW);

   // Применить минимальное магнитное поле.
   gPwmCommand = COIL_IDLE_VALUE;
   writeCoilPWM(PIN_COIL, gPwmCommand);

   gSensorReadout = analogRead(PIN_HALL_SENSOR);

   if( (gSensorReadout >= MAG_PROX_THRESHOLD_MIN) && (gSensorReadout <= MAG_PROX_THRESHOLD_MAX) )
   {
      // Переключиться в режим управления.
      gIntegral = 0;
      return MODE_CONTROL;
   }
   else if( gSensorReadout > MAG_LIMIT_MAX )
   {
      return MODE_OFF;
   }
   else
   {
      return MODE_IDLE;
   }
}

byte checkOutOfLimits(int val)
{
   if( val <= MAG_LIMIT_MIN )
   {
      return MODE_IDLE;
   }
   else if( val >= MAG_LIMIT_MAX )
   {
      return MODE_OFF;
   }

   return MODE_CONTROL;
}

byte executeControlMode()
{
   int err;
   int der;

   byte mode = MODE_CONTROL;

   // Выполнить цикл управления с частотой = 1/CTRL_LOOP_PERIOD_MS.
   if( (signed long)( millis() - gMillisCounter ) >= 0)
   {
      gMillisCounter = millis() + CTRL_LOOP_PERIOD_MS;

      digitalWrite(PIN_CTRL_INDICATOR, HIGH);

      gPrevSensorReadout = gSensorReadout;

      // Чтение датчика холла.
      gSensorReadout = analogRead(PIN_HALL_SENSOR);

      // Проверить, не выходит ли за пределы (магнит находится за пределами диапазона катушки).
      if( (mode = checkOutOfLimits(gSensorReadout)) != MODE_CONTROL )
      {
         return mode;
      }

      err = (gSetPoint - gSensorReadout);
      der = (gPrevSensorReadout - gSensorReadout);
      gIntegral += err;
      gIntegral = constrain( gIntegral, -MAX_ERR_INTEGRAL, MAX_ERR_INTEGRAL);

      gPwmCommand = gBias + (int)(gP*err) + (int)(gD*der) + (int)(gI*gIntegral);

      gPwmCommand = constrain( gPwmCommand, 0, COIL_MAX_PWM_VALUE);

      // Применить вывод.
      writeCoilPWM(PIN_COIL, gPwmCommand);

      // Запись данных в последовательный порт.
/*      Serial.write( (uint8_t)0xAF );
      Serial.write( (uint8_t)gSetPoint );
      Serial.write( (uint8_t)(gSetPoint >> 8) );
      Serial.write( (uint8_t)gSensorReadout );
      Serial.write( (uint8_t)(gSensorReadout >> 8) );
      Serial.write( gPwmCommand );
   }

   return mode;
}

void setup()
{
   setupCoilPWM();

   pinMode(PIN_CTRL_INDICATOR, OUTPUT);

   Serial.begin(115200);

   gMillisCounter = 0;
   gMode = MODE_IDLE;
   gSetPoint = CONTROL_SETPOINT;
   gBias = CONTROL_BIAS;
   gSensorReadout = 0;
   gIntegral = 0;
   gP = PGain;
   gD = DGain;
   gI = IGain;
}

void loop()
{
   // Пользовательские команды.
   if( Serial.available() )
   {
      processCommand( Serial.read() );
   }

   // Состояние.
   switch( gMode )
   {
      case MODE_OFF:
         gMode = executeOffMode();
         break;

      case MODE_IDLE:
         gMode = executeIdleMode();
         break;

      case MODE_CONTROL:
         gMode = executeControlMode();
         break;

      default:
         break;
   }
}