Ich hatte schonmal vor geraumer Zeit solch ein Programm geschrieben...
Hier der Quellcode:
Code:
//Programm zum Umfahren von Hindernissen mithilfe der Infrarot-Hinderniserkennung von waste
//zusammengestellt und programmiert von julien
#include "asuro.h"
#include <stdlib.h>
int main(void)
{
float puls;
unsigned int Rmin = 1024, Rmax = 0, Lmin = 1024, Lmax = 0, Rmitte = 512, Lmitte = 512, lspeed=150, data[2], richt=0, wegr=0, wegl=0, auswl=0, auswr=0, schritt=0, vorhe=0r;
unsigned char flagl=FALSE, flagr=FALSE, hindernis=FALSE;
Init();
DDRD |= (1 << DDD1); // Port D1 als Ausgang
PORTD &= ~(1 << PD1); // PD1 auf LOW
puls = 1;
while(1)
{
while(hindernis==FALSE){
if (PIND & (1 << PD0)){
StatusLED(GREEN); // kein Hindernis
} else {
vorher=(int)(puls);
hindernis=TRUE;
StatusLED(RED); // Hindernis erkannt
}
puls = 1.02 * puls; // Pulsbreite wird um 2% erhöht
if (puls > 10){
if (PIND & (1 << PD0)) hindernis=0;
puls = 1;
exit;
}
OCR2 = 255 - (int)(puls);
}
if(hindernis==TRUE){ //"gucken", wo noch ein freier Weg ist und diesen dann fahren
if(schritt==0){ //nach rechts gucken
MotorDir(FWD,RWD);
Msleep(500);
}
if(schritt==1){
auswr=(int)(puls);
}
if(schritt==2){ //nach links gucken
MotorDir(RWD,FWD);
Msleep(1000);
}
if(schritt==3){
auswl=(int)(puls);
MotorDir(FWD,RWD);
Msleep(500);
}
if(schritt==4){ //richtigen weg fahren
if(auswr<vorher) richt=1;
if(auswl<vorher) richt=2;
if(auswr==auswl) richt=3;
hindernis=FALSE;
schritt=0;
}
if(schritt<4) schritt++;
}
if(hindernis==FALSE){ //kein Hindernis in Sicht - geradeaus fahren
richt=0;
schritt=0;
OdometrieData(data); // 0. links, 1. rechts
// max links
if (data[0] > Lmax)
Lmax += (data[0] - Lmax) / 2;
// min links
if (data[0] < Lmin)
Lmin -= (Lmin - data[0]) / 2;
// max rechts
if (data[1] > Rmax)
Rmax += (data[1] - Rmax) / 2;
// min rechts
if (data[1] < Rmin)
Rmin -= (Rmin - data[1]) / 2;
Rmitte=(Rmax+Rmin)/2;
Lmitte=(Lmin+Lmax)/2;
if ((data[0] < Lmitte) && (flagl == TRUE)) {
flagl = FALSE;
wegl++;
}
if ((data[0] > Lmitte) && (flagl == FALSE)) {
flagl = TRUE;
wegl++;
}
if ((data[1] < Rmitte) && (flagr == TRUE)) {
flagr = FALSE;
wegr++;
}
if ((data[1] > Rmitte) && (flagr == FALSE)) {
flagr = TRUE;
wegr++;
}
if(wegl<wegr) lspeed++;
if(wegl>wegr) lspeed--;
if(wegl>=65000){ wegr=wegr-65000; } //zurücksetzen, damit kein Überlauf nach
if(wegr>=65000){ wegl=wegl-65000; } //längeren Fahrten passiert
}
if(richt==0) MotorDir(FWD,FWD); //geradeaus
if(richt==1) MotorDir(RWD,FWD); //links
if(richt==2) MotorDir(FWD,RWD); //rechts
if(richt==3) MotorDir(RWD,RWD); //zurück
MotorSpeed(lspeed,150);
}
return 0;
}
Erklärung: wenn ein Hindernis vor ihm erkannt wird, guckt er nach links und dann nach rechts und ermittelt somit, in welcher richtung das Hindernis entfernter ist. Dann fährt er in diese Richtung. Ist kein Ausweg zu erkennen, fährt er zurück.
Leider funktioniert es nicht ganz richtig. Deshalb ist es sehr verbesserungswürdig...
Kann mir bitte jemand bei diversen Verbesserungsarbeiten helfen?
MfG
Julien
Lesezeichen