wastePID.c-Dateireferenz

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

gehe zum Quellcode dieser Datei

Funktionen
void	FollowLine (void)
int	main (void)
Variablen
unsigned char	speed
int	speedLeft
int	speedRight
unsigned int	lineData [2]
int	x
int	xalt
int	don
int	doff
int	kp
int	kd
int	ki
int	yp
int	yd
int	yi
int	drest
int	y
int	y2
int	isum

---

Dokumentation der Funktionen

void FollowLine

(

void

)

Definiert in Zeile 16 der Datei wastePID.c.

{
unsigned char leftDir = FWD, rightDir = FWD;
   FrontLED(OFF);
   LineData(lineData);                  // Messung mit LED OFF
   doff = (lineData[0] - lineData[1]);   // zur Kompensation des Umgebungslicht
   FrontLED(ON);
   LineData(lineData);                  // Messung mit LED ON
   don = (lineData[0] - lineData[1]);   
   x = don - doff;                  // Regelabweichung
   isum += x;
   if (isum > 16000) isum =16000;         //Begrenzung um Überlauf zu vermeiden
   if (isum < -16000) isum =-16000;
   yi = isum/625 * ki;               //I-Anteil berechnen
   yd = (x - xalt)*kd;               // D-Anteil berechnen und mit
   yd += drest;                     // nicht berücksichtigtem Rest addieren
   if (yd > 255) drest = yd - 255;      // merke Rest
   else if (yd < -255) drest = yd + 255;
   else drest = 0;
   if (isum > 15000) BackLED(OFF,ON);   // nur zur Diagnostik
   else if (isum < -15000) BackLED(ON,OFF);
   else BackLED(OFF,OFF);
   yp = x*kp;                        // P-Anteil berechnen
   y = yp + yi + yd;                  // Gesamtkorrektur
   y2 = y/2;                        // Aufteilung auf beide Motoren
   xalt = x;                        // x merken
   speedLeft = speedRight = speed;
   MotorDir(FWD,FWD);
   if ( y > 0) {                     // nach rechts
      StatusLED(GREEN);
      speedLeft = speed + y2;         // links beschleunigen
      if (speedLeft > 255) {
         speedLeft = 255;            // falls Begrenzung
         y2 = speedLeft - speed;      // dann Rest rechts berücksichtigen
      }
      y = y - y2;
      speedRight = speed - y;         // rechts abbremsen
      if (speedRight < 0) {
         speedRight = 0;
      }
   }
   if ( y < 0) {                     // nach links
      StatusLED(RED);
      speedRight = speed - y2;         // rechts beschleunigen
      if (speedRight > 255) {
         speedRight = 255;            // falls Begrenzung
         y2 = speed - speedRight;      // dann Rest links berücksichtigen
      }
      y = y - y2;
      speedLeft = speed + y;            // links abbremsen
      if (speedLeft < 0) {
         speedLeft = 0;
      }
   }
   leftDir = rightDir = FWD;
   if (speedLeft < 20)  leftDir = BREAK; // richtig bremsen
   if (speedRight < 20) rightDir = BREAK;
   MotorDir(leftDir,rightDir);
   MotorSpeed(abs(speedLeft),abs(speedRight));
}

int main

(

void

)

Definiert in Zeile 77 der Datei wastePID.c.

{
unsigned char sw;
   Init();
   MotorDir(FWD,FWD);
   StatusLED(GREEN);
   speed = 150;
   kp = 3; ki = 10; kd = 70;      // Regler Parameter kd enthält bereits Division durch dt
   sw = PollSwitch();
   if (sw & 0x01)
      {ki=20;}
   if (sw & 0x02)
      {speed = 200;}
   if (sw & 0x04)
      speed = 100;
   if (sw & 0x08)
      kd = 35;
   if (sw & 0x10)
      kd = 70;
   if (sw & 0x20)
      kd = 140;
   FrontLED(ON);
   LineData(lineData);
   speedLeft = speedRight = speed;
   while(1){
      FollowLine();
   }
    return 0;
}

---

Variablen-Dokumentation

int doff

Definiert in Zeile 14 der Datei wastePID.c.

int don

Definiert in Zeile 14 der Datei wastePID.c.

int drest

Definiert in Zeile 14 der Datei wastePID.c.

int isum

Definiert in Zeile 14 der Datei wastePID.c.

int kd

Definiert in Zeile 14 der Datei wastePID.c.

int ki

Definiert in Zeile 14 der Datei wastePID.c.

int kp

Definiert in Zeile 14 der Datei wastePID.c.

unsigned int lineData[2]

Definiert in Zeile 13 der Datei wastePID.c.

unsigned char speed

Definiert in Zeile 11 der Datei wastePID.c.

int speedLeft

Definiert in Zeile 12 der Datei wastePID.c.

int speedRight

Definiert in Zeile 12 der Datei wastePID.c.

int x

Definiert in Zeile 14 der Datei wastePID.c.

int xalt

Definiert in Zeile 14 der Datei wastePID.c.

int y

Definiert in Zeile 14 der Datei wastePID.c.

int y2

Definiert in Zeile 14 der Datei wastePID.c.

int yd

Definiert in Zeile 14 der Datei wastePID.c.

int yi

Definiert in Zeile 14 der Datei wastePID.c.

int yp

Definiert in Zeile 14 der Datei wastePID.c.

---