Zitat von
pinsel120866
Kannst du bitte den Code mal posten oder mir ans Forum mailen, damit ich vergleichen kann?
na ja, das Programm ist jetzt an DOROs Geschwindigkeit und Größe angepasst. Ich bezweifle, dass der ASURO mit diesen Einstellungen andere Objekte genauso gut verfolgen kann.
Code:
//-------------------------------------------------------------------
//-------------------------------------------------------------------
// M1.R
// doroverfolgung_v01
// mit tasterabfrage
//-------------------------------------------------------------------
//-------------------------------------------------------------------
#include "asuro.h"
#include "myasuro.h"
//-------------------------------------------------------------------
//variable
uint16_t i;
uint8_t tasterwert, objekt_weit, objekt_nah, rechts, richtung, abbrechen;
//-------------------------------------------------------------------
// defines
#define nah 1 //1
#define weit 3 //max 16
#define speedniedrig 0 //50
#define speedhoch 200 //180
#define speedausgleich 10
#define schwenkdauer 80 //60
//-------------------------------------------------------------------
//projekt-funktionen
//-------------------------------------------------------------------
//-------------------------------------------------------------------
// wenden
void kollision (void)
{
tasterwert = PollSwitch();
FrontLED(ON);
MotorDir(RWD,RWD);
if (tasterwert > 6) // linker taster
{
BackLEDleft(ON);
BackLEDright(HALF);
MotorSpeed(160,0);
}
else
{
BackLEDleft(HALF);
BackLEDright(ON);
MotorSpeed(0,160);
}
Msleep(100);
MotorSpeed(160,160);
Msleep(300); // noch ein stückchen rückwärts
MotorDir(FWD,FWD);
abbrechen = 0;
KeyPressed();
FrontLED(OFF);
BackLEDleft(OFF);
BackLEDright(OFF);
}
//-------------------------------------------------------------------
void fahr_nach_links (void)
{
BackLEDleft(HALF);
BackLEDright(OFF);
MotorDir(FWD,FWD);
MotorSpeed(speedniedrig,speedhoch+speedausgleich);
}
//-------------------------------------------------------------------
void fahr_nach_rechts (void)
{
BackLEDleft(OFF);
BackLEDright(HALF);
MotorDir(FWD,FWD);
MotorSpeed(speedhoch,speedniedrig+speedausgleich);
}
//-------------------------------------------------------------------
//-------------------------------------------------------------------
//hauptprogramm
int main(void)
{
Init();
StatusLED(OFF);
akku_kontrolle();
//geradeaustest
/*
MotorDir(FWD,FWD);
MotorSpeed(speedniedrig,speedniedrig+speedausgleich);
Msleep(2000);
MotorSpeed(speedhoch,speedhoch+speedausgleich);
Msleep(2000);
MotorSpeed(0,0);
*/
//-------------------------------------------------------------------
// warten
while (objekt_sichtbar(weit) == 0)
{
BackLEDleft(HALF);
BackLEDright(HALF);
StatusLED(OFF);
MotorDir(FWD,FWD);
MotorSpeed(0,0);
}
//-------------------------------------------------------------------
// hauptschleife
while(1)
{
if (objekt_weit == 0)
{
StatusLED(YELLOW);
BackLEDleft(HALF);
BackLEDright(HALF);
MotorDir(FWD,FWD);
MotorSpeed(200,200);
//Msleep(100);
}
objekt_weit = objekt_sichtbar(weit);
if (KeyPressed() == 1)
{
kollision();
}
//-------------------------------------------------------------------
//wenn objekt gesehen, verfolgen!!
if (objekt_weit == 1)
{
StatusLED(GREEN);
BackLEDleft(OFF);
BackLEDright(OFF);
MotorDir(FWD,FWD);
richtung = 0;
abbrechen = 0;
if (KeyPressed() == 1)
{
kollision();
abbrechen = 1;
}
while(abbrechen == 0)
{
while ((objekt_sichtbar(weit) == 1) && (abbrechen == 0) && (objekt_sichtbar(nah) == 0))
{
StatusLED(GREEN);
if (KeyPressed() == 1)
{
kollision();
abbrechen = 1;
}
//fahr nach links
if ((objekt_sichtbar(weit) == 1) && (richtung == 0) && (abbrechen == 0))
{
i=0;
while ((abbrechen == 0) && (i<= schwenkdauer))
{
if (KeyPressed() == 1)
{
kollision();
abbrechen = 1;
}
fahr_nach_links ();
richtung=1;
i++;
Msleep(1);
}
i=0;
}
//fahr nach rechts
if ((objekt_sichtbar(weit) == 1) && (richtung == 1) && (abbrechen == 0))
{
i=0;
while ((abbrechen == 0) && (i<=schwenkdauer))
{
if (KeyPressed() == 1)
{
kollision();
abbrechen = 1;
}
fahr_nach_rechts ();
richtung=0;
i++;
Msleep(1);
}
i=0;
}
}
//wenn kein objekt mehr zu sehen ist
if ((objekt_sichtbar(weit) == 0) && (abbrechen == 0))
{
//wenn letzte richtung nach rechts war
//dann ist das objekt links verloren gegangen
//linke backled an
//nach links fahren bis objekt wieder da ist
StatusLED(YELLOW);
BackLEDleft(OFF);
BackLEDright(OFF);
if (richtung == 0)
{
i = 0;
while ((objekt_sichtbar(weit) == 0) && (abbrechen == 0))
{
if (KeyPressed() == 1)
{
kollision();
abbrechen = 1;
}
//ein stück nach links
fahr_nach_links ();
//Msleep(150);
if (objekt_sichtbar(weit) == 1) //und noch ein stück nach links
{
fahr_nach_links ();
Msleep(150);
}
richtung=0; //danach mit links anfangen
Msleep(1);
}
abbrechen = 1;
}
//wenn letzte richtung nach links war
//dann ist das objekt rechts verloren gegangen
//rechte backled an
//nach rechts fahren bis objekt wieder da ist
//oder nach einer gewissen zeit nicht wieder aufgetaucht ist
else if (richtung == 1) //letzte richtung war nach links
{
i = 0;
while ((objekt_sichtbar(weit) == 0) && (abbrechen == 0))
{
if (KeyPressed() == 1)
{
kollision();
abbrechen = 1;
}
//ein stück nach rechts
fahr_nach_rechts();
//Msleep(150);
if (objekt_sichtbar(weit) == 1) //und noch ein stück nach rechts
{
fahr_nach_rechts ();
Msleep(150);
}
richtung=1; //danach mit rechts anfangen
Msleep(1);
}
abbrechen = 1;
StatusLED(OFF);
BackLEDleft(OFF);
BackLEDright(OFF);
}
}
//wenn objekt ganz nah, bremsen
if (objekt_sichtbar(nah) == 1)
{
StatusLED(RED);
BackLEDleft(OFF);
BackLEDright(OFF);
MotorDir(BREAK,BREAK);
if (KeyPressed() == 1)
{
kollision();
abbrechen = 1;
}
}
}
}
//-------------------------------------------------------------------
}
while (1);
return 0;
}
Gruss
M.
Zitieren
Lesezeichen