Waste,
ich habe Deinen Code leicht modifiziert, da Dein Programm bei mir keiner Linie folgte.
Folgende Aenderungen habe ich vorgenommen:
1.) die Variablen drest, xsum und xalt in MAIN, da diese Variablen sonst nicht initialisiert werden
2.) Linedata mit LED off zur Initialisierung von doff in MAIN, da bei mir die LED beim Ein- und Ausschalten in FollowLine zu traege ist und scheinbar immer an bleibt.
3.) Implementierung mit Lib Ver. 2.70
Das war's. Nun folgt mein Asuro brav der Linie und der PID-Regler ist viel sensibler als der einfache P-Regelkreis!
Ich habe noch eine Frage:
Ich verstehe die Logik beim abbremsen nicht.
Links von der Linie (rechts bremsen)ist der Code:
speedRight = speed - y; // rechts abbremsen
Links dagegen
speedLeft = speed + y; // links abbremsen
Kannst Du mir das mal erklaeren. Besten Dank im voraus.
Hier ist der neue Code:
Code:
/*******************************************************************************
*
* Description: Asuro Linienverfolgung mit PID-Regler
* Version 1: Korrektur auf beide Motoren verteilt
* Author: Waste 26.8.05
*
* Version 2: initialization of parameters in Main, measurement with LED off in Main
* Author: irobot22587 11.03.2007
*
*****************************************************************************/
#include "c:/WinAVR/avr/include/asuro.h" // ver 2.70
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;
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 !!!was speed - y2!!
if (speedRight > 255) {
speedRight = 255; // falls Begrenzung
y2 = speed - speedRight; // dann Rest links berücksichtigen !!was speed - speedRight!!
}
y = y - y2;
speedLeft = speed + y; // links abbremsen !! was speed + y!!!
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
drest=0;
isum=0;
xalt=0;
sw=0;
/*
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(OFF);
Msleep(50);
LineData(lineData); // Messung mit LED OFF
doff = (lineData[0] - lineData[1]); // zur Kompensation des Umgebungslicht
FrontLED(ON);
Msleep(100);
speedLeft = speed;
speedRight = speed+25;
while(1){
FollowLine();
}
return 0;
}
Zitieren
Zitat von irobot_22587
also leg los, es hält dich bestimmt keiner davon ab das zu machen.
Berechtigungen
Lesezeichen