Liste der Anhänge anzeigen (Anzahl: 4)
Problem mit PID-Regler
Hi,
Ich habe für ein Linienfolger Programm einen PID Regler verwendet (von http://www.henkessoft.de/Roboter/ASURO.htm), dieser Funktionierte auch wunderbar und war viel schneller und Präziser als ein einfacher P-Regler.
Code:
/*******************************************************************************
*
* Description: Asuro Linienverfolgung mit PID-Regler
* Autor: Waste 26.8.05, Verändert durch: ehenkes 26.05.2007
*
*****************************************************************************/
#include "asuro.h"
#include <stdlib.h>
#define SPEED 80
#define SPEEDMAX 200
#define SPEEDMIN 30
#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;
}
Um aber flexibler zu sein habe ich meinen Asuro ein wenig umgebaut:
----------------------------------------------
|_|''''''''''''''''''''''''''''''|_|'''''''''''''' '''''''''''''''|_|
(schematische Ansicht von vorne; linker Fototransistor, FrontLED, rechter Fototransistor)
Den Code habe ich nun folgendermassen angepasst:
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 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(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));
}
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(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;
}
Als wichtigste Änderung habe ich die Umkehrung der > und < Operatoren betrachtet.
Nun fährt der Asuro aber nur noch sehr langsam der Linie nach...
Kann mir jemand einen Tipp geben, wie ich die Geschwindigkeit wieder steigern kann?
Gruss,
Matt
Edit:
Bilder hinzugefügt
Liste der Anhänge anzeigen (Anzahl: 2)
Das mit den Reglerwerten habe ich schon zur genüge ausgetestet.. Ich frage mich nur, ob ich nicht etwas vergessen habe. Ich habe schon wie ein verrückter gesucht, bin aber noch nicht fündig geworden.
Anbei liefere ich euch noch ein paar bilder mit, mit denen ihr hoffentlich meine situation nachvollziehen könnt.
Durch meinen Umbau ist der Spielraum der breite des Tesafilms grösser, da sich die Fototransistoren nun nicht mehr auf, sondern neben dem Klebeband befinden..
Die von mir beschriebene Situation (die 90 grad kurve), seht ihr auf meinen Bildern; dieses Problem ist durch den Umbau gelöst worden.