Hallo
Also ich hab da ein Problem mit meiner Odometrie.
Ich hab grösstenteils den Code von liggi übernommen und ein wenig verändert.
Ich poste jetzt nur mal die Funktion und die definierten Werte, sonst wird das so unübersichtlich.
Was der Code machen soll:
Der Asuro soll geradeausfahren: OK
Sollte ein Taster gedrückt werden soll er aus der Funktion springen und entsprechend reagieren: OK
Wenn er stehen bleibt soll er dies erkennen und reagieren:
Und genau das funktioniert nur teilweise. Wenn ich ihn beim Fahren mit dem Finger stoppe, passiert gar nichts. Erst wenn ich ihn ein paar Ticks mit dem Zahnrad machen lasse erkennt er, dass da was nicht stimmt und fährt zurück.
Könnt ihr euch den Code vll. mal durchgucken und mir sagen wo's hakt?
Bleibt die Schleife irgendwo hängen? Weil eigentlich sollte die ja die ganze Zeit laufen und dann auch ständig prüfen, ob der Asuro steht.
LG Nils
Code:
#define TRIGGERLEVEL 600
#define PUFFER 3
#define COUNTERABFRAGE 40
#define LOW 0
#define HIGH 1
#define SPEED 120
int odometrie()
{
unsigned int data[2]; // für die Odometrie Daten
signed int status[2] = {0, 0}; // vorheriger Status der Odometrie links und rechts
signed int difference = 0;
int weg = 0; // Bei jedem Wechsel der Odometrie wird weg für beide Räder erhöht.
int weg_alt = 0; // Zwischenspeichern des vorherigen gefahrenen Weges
int counter = 0; // Wenn counter einen bestimmten Wert (COUNTERABFRAGE) erreicht, wird weg mit weg_alt verglichen und entsprechend reagiert
MotorDir(FWD,FWD);
MotorSpeed(SPEED,SPEED);
while(PollSwitch() == 0)
{
OdometrieData(data);
if ((status[0] == LOW) && (data[0] < TRIGGERLEVEL)) /*Wenn linker Sensor von niedrig auf hoch wechselt*/
{
status[0] = HIGH;
difference++;
weg++;
}
if ((status[0] == HIGH) && (data[0] > TRIGGERLEVEL)) /*Wenn linker Sensor von hoch auf niedrig wechselt*/
{
status[0] = LOW;
difference++;
weg++;
}
if ((status[1] == LOW) && (data[1] < TRIGGERLEVEL)) /*Wenn rechter Sensor von niedrig auf hoch wechselt*/
{
status[1] = HIGH;
difference--;
weg++;
}
if ((status[1] == HIGH) && (data[1] > TRIGGERLEVEL)) /*Wenn rechter Sensor von hoch auf niedrig wechselt*/
{
status[1] = LOW;
difference--;
weg++;
}
if (difference < !SPEED) /*Sicherheitsabfragen*/
{
difference = !SPEED;
}
if (difference > SPEED)
{
difference = SPEED;
}
if (difference > 0)
{
MotorSpeed(SPEED - difference, SPEED); /*Motoren einstellen*/
}
else if (difference < 0)
{
MotorSpeed(SPEED, SPEED + difference);
}
if (SPEED - difference <= 100)
{
difference=0;
}
counter++;
if (counter == COUNTERABFRAGE)
{
counter = 0;
if (weg - PUFFER <= weg_alt)
{
weg_alt = 0;
weg = 0;
// Zurückfahren mit einer anderen Funktion
}
else
{
weg_alt = weg;
}
}
}
}
[/code]
Lesezeichen