Code:
#include "asuro.h"
#include <stdlib.h>
#define SPEED 100
#define SPEEDMAX 200
#define SPEEDMIN 20
#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;
}
Lesezeichen