Ich habe ja ein Linienprogramm.
Mein Asuro soll die Linie entlang fahren und soll am Ende umkehren und die Linie wider zurück fahre.
Mein Quell Code für die Linien Verfolgung:
Code:
#include "asuro.h"
#include <stdlib.h>
unsigned char speed;
int speedLeft,speedRight;
unsigned int lineData[2],zeit=0;
int x, xalt, don, doff, kp, kd, ki, yp, yd, yi, drest, y, y2, isum;
int x1, x2, x3, x4;
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]);
x1 = don - doff; // Regelabweichung
x = (x1+x2+x3+x4)/4; //Filter
x4=x3; x3=x2; x2=x1;
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 = 5; ki = 5; kd = 70; // Regler Parameter kd enthält bereits Division durch dt
sw = PollSwitch();
if (sw & 0x01)
{ki=5;}
if (sw & 0x02)
{speed = 200;}
if (sw & 0x04)
speed = 255;
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();
StatusLED(GREEN); // zeigt durchlauf der Schleaife
FollowLine();
StatusLED(RED);
zeit++;
if(zeit>100) break; //
}
MotorSpeed(0,0);
return 0;
}
Mit der Status LED wollte ich anzeigen lassen ob er die Schleife auch wirklich durchläuft.
Macht er auch.
Blos mit dem break Befehl das funktioniert nicht.
Wie gesagt mit Sleep habe ich es auch schon probiert aber das ging auch nicht.
Ich weis jetzt auch nicht mehr.
Lesezeichen