Regulator PD Micromouse

[Bleebum]

Witam, 

 Względem dwóch ścian mysz porusza się prosto, ale względem lewej lub prawej już nie. Wydaje mi się, że to wina czujników (Sharp GP2Y0A41SK0F, zakres 4-30cm), poniżej 4cm zwracają wartości   które są większe od 66 i wtedy błąd nie jest poprawnie obliczany. Za pomocą enkoderów mysz też nie jedzie prosto, przy większym współczynniku drastycznie skręca w prawo. W załączniku wysyłam odczyty z enkoderów co 100 ms. Także co 100 ms zwiększana jest prędkość (PWM) silników co 2 do 50.

void PID()
{
  nowTime = millis();
  uint16_t timeChange = nowTime - lastTime;
  int errorP = 0, errorD = 0;

  float Kp = 0.2;
  float Kd = 0.1;

  if(timeChange > 100) 
  {
    
    if(sensorArray[2][0] != 0 && sensorArray[2][0] < 100 && sensorArray[2][2] != 0 && sensorArray[2][2] < 100)
    {
      errorP = sensorArray[2][2] - sensorArray[2][0];
      errorD = errorP - oldError;

    }  else if(sensorArray[2][0] > 66 && sensorArray[2][2] == 0 )
    {
      errorP = sensorArray[2][0] - 66;
      errorD = 0;
      Kp = 0.1;
    } else if(sensorArray[2][2] > 66 && sensorArray[2][0] == 0 )
    {
      errorP = 66 - sensorArray[2][2];
      errorD = 0;
      Kp = 0.1;
    } else 
    {
      errorP = (abs(lticks) - abs(rticks));
      errorD = 0;
      Kp = 0.1;
    }

    output = Kp * errorP + Kd * errorD;
    oldError = errorP;
    
    lastTime = nowTime;
    totalTicks += (abs(lticks)+abs(rticks))/2;
    lticks = 0;
    rticks = 0;

    output = constrain(output, -15, 15);

    updateMotors();  
  }
  
}

 

sensorArray[2][0] - lewy czujnik, sensorArray[2][2] - prawy czujnik.

Proszę o pomoc 🙂 

 

enkoder.pdf