wastePID.c

gehe zur Dokumentation dieser Datei

 /*******************************************************************************
*
* Description: Asuro Linienverfolgung mit PID-Regler
* Version 1: Korrektur auf beide Motoren verteilt
* Autor: Waste   26.8.05
*
*****************************************************************************/
#include "asuro.h"
#include <stdlib.h>

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

void FollowLine (void)
{
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)
{
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;
}

---