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;
}