-
Hi!
Ich hab hier noch einen weiteren Code, der nicht richtig läuft und der für euch wahrscheinlich einfacher nachzuvollziehen ist, da er nichts mit dem Umbau zu tun hat.
Und zwar handelt es sich um einen PID Regler, der mittels Odometrie die Geradeaus-fahrt regeln soll.
Das Programm beruht auf dem zuvor geposteten LinienPID-Regler.
Ich bin gespannt auf eure Vorschläge!
Gruss,
Matt
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 DrehDataLeft, DrehDataRight;
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;
OdoLeft();
OdoRight();
x = (DrehDataLeft - DrehDataRight) - ADOffset;
SerWrite("\n\rBeginn FollowLine(void)... Wert von x: ", 41);
PrintInt(x);
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));
}
void OdoLeft(void)
{
unsigned long zeitbeginn, zeitende, zeitdiff;
unsigned int OdoData[2];
int count, l_old;
Init();
SerWrite("\n\rBeginn OdoLeft()", 20);
zeitbeginn = Gettime();
for(count = 0; count < 6;)
{
OdometrieData(OdoData);
if (OdoData[0] > 800 && l_old < 800)
{
count++;
}
l_old = OdoData[0];
}
count = 0;
zeitende = Gettime();
zeitdiff = zeitende - zeitbeginn;
DrehDataLeft = 60000 / zeitdiff;
SerWrite("\n\rEnde OdoLeft(), wert von DrehDataLeft ", 43);
PrintInt(DrehDataLeft);
}
void OdoRight(void)
{
unsigned long zeitbeginn, zeitende, zeitdiff;
unsigned int OdoData[2], drehzahl_right;
int count, r_old;
Init();
SerWrite("\n\rBeginn OdoRight()", 21);
zeitbeginn = Gettime();
for(count = 0; count < 6;)
{
OdometrieData(OdoData);
if (OdoData[1] > 600 && r_old < 600)
{
count++;
}
r_old = OdoData[1];
}
count = 0;
zeitende = Gettime();
zeitdiff = zeitende - zeitbeginn;
DrehDataRight = 60000 / zeitdiff;
SerWrite("\n\rEnde OdoRight(), wert von DrehDataRight ", 43);
PrintInt(DrehDataRight);
}
int main(void)
{
Init();
FrontLED(ON);
MotorDir(RWD,RWD);
MotorSpeed(SPEED,SPEED);
SerWrite("Beginn main(void)", 17);
OdoLeft();
OdoRight();
SerWrite("\n\rDrehDataLeft, DrehDataRight ", 30);
PrintInt(DrehDataLeft);
SerWrite(" ", 1);
PrintInt(DrehDataRight);
OdoLeft();
OdoRight();
ADOffset = DrehDataLeft - DrehDataRight; // 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;
}
-
achso... ich dachte es geht bloss darum dass er eine sschmale linie nicht mehr erkennen würde wenn diese einen scharfen knick macht...
-
Soo... Überraschung ;)
Für alle die's interessiert... Ich habe den Geradeaus PID Regler fertig. Es lag bloss an den überzähligen Init(); aufrufen.
Hier der fertige code:
Code:
include "asuro.h"
#include <stdlib.h>
#define SPEED 180
#define SPEEDMAX 255
#define SPEEDMIN 80
#define IMAX 16000
#define IMAXALARM 15000
unsigned char speed, j;
int speedLeft,speedRight;
unsigned int DrehDataLeft, DrehDataRight;
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;
OdoLeft();
OdoRight();
x = (DrehDataLeft - DrehDataRight) - ADOffset;
SerWrite("\n\rBeginn FollowLine(void)... Wert von x: ", 41);
PrintInt(x);
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));
}
void OdoLeft(void)
{
unsigned long zeitbeginn, zeitende, zeitdiff;
unsigned int OdoData[2];
int count, l_old;
SerWrite("\n\rBeginn OdoLeft()", 20);
zeitbeginn = Gettime();
for(count = 0; count < 6;)
{
OdometrieData(OdoData);
if (OdoData[0] > 800 && l_old < 800)
{
count++;
}
l_old = OdoData[0];
}
count = 0;
zeitende = Gettime();
zeitdiff = zeitende - zeitbeginn;
DrehDataLeft = 60000 / zeitdiff;
SerWrite("\n\rEnde OdoLeft(), wert von DrehDataLeft ", 43);
PrintInt(DrehDataLeft);
}
void OdoRight(void)
{
unsigned long zeitbeginn, zeitende, zeitdiff;
unsigned int OdoData[2], drehzahl_right;
int count, r_old;
SerWrite("\n\rBeginn OdoRight()", 21);
zeitbeginn = Gettime();
for(count = 0; count < 6;)
{
OdometrieData(OdoData);
if (OdoData[1] > 600 && r_old < 600)
{
count++;
}
r_old = OdoData[1];
}
count = 0;
zeitende = Gettime();
zeitdiff = zeitende - zeitbeginn;
DrehDataRight = 60000 / zeitdiff;
SerWrite("\n\rEnde OdoRight(), wert von DrehDataRight ", 43);
PrintInt(DrehDataRight);
}
int main(void)
{
Init();
FrontLED(ON);
MotorDir(RWD,RWD);
MotorSpeed(SPEED,SPEED);
SerWrite("Beginn main(void)", 17);
OdoLeft();
OdoRight();
SerWrite("\n\rDrehDataLeft, DrehDataRight ", 30);
PrintInt(DrehDataLeft);
SerWrite(" ", 1);
PrintInt(DrehDataRight);
OdoLeft();
OdoRight();
ADOffset = DrehDataLeft - DrehDataRight; // 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;
}
Über optiemierungsvorschläge würde ich mich sehr freuen!
Gruss,
Matt
