Problem mit PID-Regler

**mattsarz** · 04.10.2007, 14:21

Hi!

Ich hab hier noch einen weiteren Code, der nicht richtig läuft und der für euch wahrscheinlich einfacher nachzuvollziehen ist, da er nichts mit dem Umbau zu tun hat.
Und zwar handelt es sich um einen PID Regler, der mittels Odometrie die Geradeaus-fahrt regeln soll.
Das Programm beruht auf dem zuvor geposteten LinienPID-Regler.
Ich bin gespannt auf eure Vorschläge!

Gruss,
Matt

Code:

#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 DrehDataLeft, DrehDataRight;
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;
  
   OdoLeft();
   OdoRight();
   x = (DrehDataLeft - DrehDataRight) - ADOffset;
   
   SerWrite("\n\rBeginn FollowLine(void)... Wert von x: ", 41);
   PrintInt(x);
    
   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));
}



void OdoLeft(void)
{
  unsigned long zeitbeginn, zeitende, zeitdiff;
  unsigned int OdoData[2];
  int count, l_old;

  Init();

	SerWrite("\n\rBeginn OdoLeft()", 20);
    zeitbeginn = Gettime();
      for(count = 0; count < 6;)
	{
        OdometrieData(OdoData);
        if (OdoData[0] > 800 && l_old < 800) 
        {
            count++;
        }
		l_old = OdoData[0];
	}
    count = 0;
    zeitende = Gettime();
	zeitdiff = zeitende - zeitbeginn;
    DrehDataLeft = 60000 / zeitdiff;
    SerWrite("\n\rEnde OdoLeft(), wert von DrehDataLeft ", 43);
    PrintInt(DrehDataLeft);
}



void OdoRight(void)
{
  unsigned long zeitbeginn, zeitende, zeitdiff;
  unsigned int OdoData[2], drehzahl_right;
  int count, r_old;

  Init();

	SerWrite("\n\rBeginn OdoRight()", 21);
    zeitbeginn = Gettime();
      for(count = 0; count < 6;)
	{
        OdometrieData(OdoData);
        if (OdoData[1] > 600 && r_old < 600) 
        {
            count++;
        }
		r_old = OdoData[1];
	}
    count = 0;
    zeitende = Gettime();
	zeitdiff = zeitende - zeitbeginn;
    DrehDataRight = 60000 / zeitdiff;
    SerWrite("\n\rEnde OdoRight(), wert von DrehDataRight ", 43);
    PrintInt(DrehDataRight);
}



int main(void)
{
   Init();
   FrontLED(ON);
   MotorDir(RWD,RWD);
   MotorSpeed(SPEED,SPEED);
    SerWrite("Beginn main(void)", 17);
    OdoLeft();
    OdoRight();
   SerWrite("\n\rDrehDataLeft, DrehDataRight ", 30);
   PrintInt(DrehDataLeft);
   SerWrite(" ", 1);
   PrintInt(DrehDataRight);
   OdoLeft();
   OdoRight();
   ADOffset = DrehDataLeft - DrehDataRight; // 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;
}

**damaltor** · 04.10.2007, 22:41

achso... ich dachte es geht bloss darum dass er eine sschmale linie nicht mehr erkennen würde wenn diese einen scharfen knick macht...

**mattsarz** · 05.10.2007, 09:38

Soo... Überraschung

Für alle die's interessiert... Ich habe den Geradeaus PID Regler fertig. Es lag bloss an den überzähligen Init(); aufrufen.

Hier der fertige code:

Code:

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

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

unsigned char speed, j; 
int speedLeft,speedRight; 
unsigned int DrehDataLeft, DrehDataRight; 
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; 
  
   OdoLeft(); 
   OdoRight(); 
   x = (DrehDataLeft - DrehDataRight) - ADOffset; 
    
   SerWrite("\n\rBeginn FollowLine(void)... Wert von x: ", 41); 
   PrintInt(x); 
    
   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)); 
} 



void OdoLeft(void) 
{ 
  unsigned long zeitbeginn, zeitende, zeitdiff; 
  unsigned int OdoData[2]; 
  int count, l_old; 


   SerWrite("\n\rBeginn OdoLeft()", 20); 
    zeitbeginn = Gettime(); 
      for(count = 0; count < 6;) 
   { 
        OdometrieData(OdoData); 
        if (OdoData[0] > 800 && l_old < 800) 
        { 
            count++; 
        } 
      l_old = OdoData[0]; 
   } 
    count = 0; 
    zeitende = Gettime(); 
   zeitdiff = zeitende - zeitbeginn; 
    DrehDataLeft = 60000 / zeitdiff; 
    SerWrite("\n\rEnde OdoLeft(), wert von DrehDataLeft ", 43); 
    PrintInt(DrehDataLeft); 
} 



void OdoRight(void) 
{ 
  unsigned long zeitbeginn, zeitende, zeitdiff; 
  unsigned int OdoData[2], drehzahl_right; 
  int count, r_old; 


   SerWrite("\n\rBeginn OdoRight()", 21); 
    zeitbeginn = Gettime(); 
      for(count = 0; count < 6;) 
   { 
        OdometrieData(OdoData); 
        if (OdoData[1] > 600 && r_old < 600) 
        { 
            count++; 
        } 
      r_old = OdoData[1]; 
   } 
    count = 0; 
    zeitende = Gettime(); 
   zeitdiff = zeitende - zeitbeginn; 
    DrehDataRight = 60000 / zeitdiff; 
    SerWrite("\n\rEnde OdoRight(), wert von DrehDataRight ", 43); 
    PrintInt(DrehDataRight); 
} 



int main(void) 
{ 
   Init(); 
   FrontLED(ON); 
   MotorDir(RWD,RWD); 
   MotorSpeed(SPEED,SPEED); 
    SerWrite("Beginn main(void)", 17); 
    OdoLeft(); 
    OdoRight(); 
   SerWrite("\n\rDrehDataLeft, DrehDataRight ", 30); 
   PrintInt(DrehDataLeft); 
   SerWrite(" ", 1); 
   PrintInt(DrehDataRight); 
   OdoLeft(); 
   OdoRight(); 
   ADOffset = DrehDataLeft - DrehDataRight; // 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; 
}

Über optiemierungsvorschläge würde ich mich sehr freuen!

Gruss,
Matt

Thema: Problem mit PID-Regler

Themen-Optionen

Thema durchsuchen

Anzeige

Lesezeichen

Lesezeichen

Berechtigungen