Problem mit PID-Regler

[mattsarz]

Hi,

Ich habe für ein Linienfolger Programm einen PID Regler verwendet (von http://www.henkessoft.de/Roboter/ASURO.htm), dieser Funktionierte auch wunderbar und war viel schneller und Präziser als ein einfacher P-Regler.

/*******************************************************************************
*
* Description: Asuro Linienverfolgung mit PID-Regler
* Autor: Waste   26.8.05,  Verändert durch: ehenkes 26.05.2007
*
*****************************************************************************/

#include "asuro.h"
#include <stdlib.h>

#define SPEED        80
#define SPEEDMAX    200
#define SPEEDMIN     30
#define IMAX      16000
#define IMAXALARM 15000

unsigned char speed, j;
int speedLeft,speedRight;
unsigned int lineData[2];
int x, xalt, kp, kd, ki, yp, yd, yi, drest=0, y, y2, isum=0, ADOffset;

void FollowLine (void)
{
   unsigned char leftDir = FWD, rightDir = FWD;
  
   LineData(lineData);
   x = (lineData[LEFT] - lineData[RIGHT]) - ADOffset;
  
   yp = x*kp;                          // P-Anteil
  
   isum += x;
   if (isum >  IMAX) isum =  IMAX;  
   if (isum < -IMAX) isum = -IMAX;
   yi = isum / 625 * ki;               //I-Anteil
  
   yd = (x - xalt) * kd;               // D-Anteil 
   yd += drest;                       
   if (yd > 255)
   {
       drest = yd - 255;   
   }
   else if (yd < -255)
   {
       drest = yd + 255;
   }
   else
   {
       drest = 0;
   }
  
   if (isum > IMAXALARM)        BackLED(OFF,ON);  
   else if (isum < -IMAXALARM)  BackLED(ON,OFF);
   else BackLED(OFF,OFF);
  
   y = yp + yi + yd;                 // PID
   y2 = y / 2;                        
   xalt = x;                        
  
   speedLeft = speedRight = speed;
   MotorDir(FWD,FWD);
  
   if ( y > 0)
   {                    
      StatusLED(GREEN);
      speedLeft = speed + y2;        
      if (speedLeft > SPEEDMAX)
      {
         speedLeft = SPEEDMAX;       
         y2 = speedLeft - speed;     
      }
      y = y - y2;
      speedRight = speed - y;        
      if (speedRight < SPEEDMIN)
      {
         speedRight = SPEEDMIN;
      }
   }
  
   if ( y < 0)
   {                    
      StatusLED(RED);
      speedRight = speed - y2;       
      if (speedRight > SPEEDMAX)
      {
         speedRight = SPEEDMAX;      
         y2 = speed - speedRight;    
      }
      y = y - y2;
      speedLeft = speed + y;         
      if (speedLeft < SPEEDMIN)
      {
         speedLeft = SPEEDMIN;
      }
   }
   leftDir = rightDir = FWD;
   if (speedLeft  < SPEEDMIN + 5)  leftDir  = BREAK;
   if (speedRight < SPEEDMIN + 5)  rightDir = BREAK;
   MotorDir(leftDir,rightDir);
   MotorSpeed(abs(speedLeft),abs(speedRight));
}

int main(void)
{
   Init();
   FrontLED(ON);
   for (j = 0; j < 255; j++) LineData(lineData);
   LineData(lineData);
   ADOffset = lineData[LEFT] - lineData[RIGHT]; // Helligkeitsunterschied links und rechts    
  
   MotorDir(FWD,FWD);
   StatusLED(GREEN);

   speed = SPEED;
   speedLeft  = speed;
   speedRight = speed; 
  
   kp = 10; ki = 4; kd = 70;      // Regler Parameter kd enthält bereits Division durch dt
   /*   10(!)    4(!)    70(!)   */ /*<=== gute Ausgangswerte*/  
  
   while(1)
   {
      FollowLine();
   }
   return 0;
}

Um aber flexibler zu sein habe ich meinen Asuro ein wenig umgebaut:

----------------------------------------------

|_|''''''''''''''''''''''''''''''|_|'''''''''''''' '''''''''''''''|_|

(schematische Ansicht von vorne; linker Fototransistor, FrontLED, rechter Fototransistor)

Den Code habe ich nun folgendermassen angepasst:

#include "asuro.h"
#include <stdlib.h>

#define SPEED       240
#define SPEEDMAX    255
#define SPEEDMIN     80
#define IMAX      16000
#define IMAXALARM 15000

unsigned char speed, j;
int speedLeft,speedRight;
unsigned int lineData[2];
int x, xalt, kp, kd, ki, yp, yd, yi, drest=0, y, y2, isum=0, ADOffset;

void FollowLine (void)
{
   unsigned char leftDir = FWD, rightDir = FWD;
  
   LineData(lineData);
   x = (lineData[LEFT] - lineData[RIGHT]) - ADOffset;
  
   yp = x*kp;                          // P-Anteil
  
   isum += x;
   if (isum >  IMAX) isum =  IMAX;  
   if (isum < -IMAX) isum = -IMAX;
   yi = isum / 625 * ki;               //I-Anteil
  
   yd = (x - xalt) * kd;               // D-Anteil 
   yd += drest;                       
   if (yd > 255)
   {
       drest = yd - 255;   
   }
   else if (yd < -255)
   {
       drest = yd + 255;
   }
   else
   {
       drest = 0;
   }
  
   if (isum > IMAXALARM)        BackLED(OFF,ON);  
   else if (isum < -IMAXALARM)  BackLED(ON,OFF);
   else BackLED(OFF,OFF);
  
   y = yp + yi + yd;			// PID
   y2 = y / 2;                        
   xalt = x;                        
  
   speedLeft = speedRight = speed;
   MotorDir(RWD,RWD);
  
   if ( y < 0)
   {                    
      StatusLED(GREEN);
      speedLeft = speed + y2;        
      if (speedLeft > SPEEDMAX)
      {
         speedLeft = SPEEDMAX;       
         y2 = speedLeft - speed;     
      }
      y = y - y2;
      if (speedRight < SPEEDMIN)
      {
         speedRight = SPEEDMIN;
      }
   }
  
   if ( y > 0)
   {                    
      StatusLED(RED);
      speedRight = speed - y2;       
      if (speedRight > SPEEDMAX)
      {
         speedRight = SPEEDMAX;      
         y2 = speed - speedRight;    
      }
      y = y - y2;
      speedLeft = speed + y;         
      if (speedLeft < SPEEDMIN)
      {
         speedLeft = SPEEDMIN;
      }
   }
   leftDir = rightDir = RWD;
   if (speedLeft  < SPEEDMIN + 5)  leftDir  = BREAK;
   if (speedRight < SPEEDMIN + 5)  rightDir = BREAK;
   MotorDir(leftDir,rightDir);
   MotorSpeed(abs(speedLeft),abs(speedRight));
}

int main(void)
{
   Init();
   FrontLED(ON);
   for (j = 0; j < 255; j++) 
   {
	   LineData(lineData);
   }
   LineData(lineData);
   ADOffset = lineData[LEFT] - lineData[RIGHT]; // Helligkeitsunterschied links und rechts    
  
   MotorDir(RWD,RWD);
   StatusLED(GREEN);

   speed = SPEED;
   speedLeft  = speed;
   speedRight = speed; 
  
   kp = 10; ki = 4; kd = 70;      // Regler Parameter kd enthält bereits Division durch dt
   
  
   while(1)
   {
      FollowLine();
   }
   return 0;
}

Als wichtigste Änderung habe ich die Umkehrung der > und < Operatoren betrachtet.

Nun fährt der Asuro aber nur noch sehr langsam der Linie nach...
Kann mir jemand einen Tipp geben, wie ich die Geschwindigkeit wieder steigern kann?

Gruss,
Matt

Edit:
Bilder hinzugefügt