Hallo,
ich habe das Programm von M1.R mal an die Objektverfolgung angepasst. Funktioniert ganz gut, allerdings verliert der ASURO leicht das vorfahrende Objekt. Möglicherweise braucht es doch noch eine zusätzliche IR-LED, damit die Sache besser funktioniert.
Code:
#include "asuro.h"
#define HALF 2
uint16_t i, zuf;
uint8_t objekt_weit, objekt_nah, rechts, links, speed1, speed2, richtung;
//abstand für ir-messung
#define nah 1 //1
#define weit 7 //max 16
#define schwenkdauer 100
#define verlierdauer 500
//-------------------------------------------------------------------
//projekt-funktionen
//-------------------------------------------------------------------
void BackLEDleft(uint8_t status);
void BackLEDright(uint8_t status);
uint8_t objekt_sichtbar(uint8_t abstand);
//------------------------------------------------------------------
//backled funktionen um die leds unabhängig voneinander
//hell leuchten oder glimmen zu lassen
//------------------------------------------------------------------
// rechte led
void BackLEDright(uint8_t status) // aus - hell - glimm
{
PORTD &= ~(1 << PD7); //odoleds aus
if (status == OFF)
{ //rechte backled aus
DDRC |= (1 << PC0); //rechts auf out
PORTC &= ~(1 << PC0); //rechte aus
}
if (status == ON)
{
//rechte backled hell
DDRC |= (1 << PC0); //rechts auf out
PORTC |= (1 << PC0); //rechte an
}
if (status == HALF)
{ //rechte backled glimmt
DDRC &= ~(1 << PC0); //rechts auf in
}
}
//------------------------------------------------------------------
// linke led
void BackLEDleft(uint8_t status) // aus - hell - glimm
{
PORTD &= ~(1 << PD7); //odoleds aus
if (status == OFF)
{ //rechte backled aus
DDRC |= (1 << PC1); //links auf out
PORTC &= ~(1 << PC1); //linke aus
}
if (status == ON)
{
//rechte backled hell
DDRC |= (1 << PC1); //links auf out
PORTC |= (1 << PC1); //linke an
}
if (status == HALF)
{ //rechte backled glimmt
DDRC &= ~(1 << PC1); //links auf in
}
}
/*************************************************************************
uint8_t objekt_sichtbar(uint8_t abstand)
Ist ein Objekt in der Entfernung kleiner oder gleich
dem Eingangsparameter "abstand" erkennbar?
objekt_sichtbar(7) liefert TRUE zurück, falls überhaupt
ein Object detektierbar.
abstand:
0: 5cm
1: 7cm
2: 13cm
3: 15cm
4: 16cm
5: 17cm
6: 18cm
7: 22cm
( Testobjekt: Joghurtecher, Umgebungsbeleuchtung: Zimmer )
return: TRUE falls Objekt gefunden
FALSE wenn nicht
------------------------------------
geändert (m1.r):
schaltet nach dem messen die led aus
und wartet noch 1ms wegen
der AGC(automatic gain control,
automatische Verstärkungsregelung) des empfängers
------------------------------------
Zeitbedarf: 6ms
author: robo.fr, christoph ( ät ) roboterclub-freiburg.de
date: 2008
*************************************************************************/
uint8_t objekt_sichtbar(uint8_t distance)
{
uint16_t j,z;
DDRD |= (1 << DDD1); // Port D1 als Ausgang
PORTD &= ~(1 << PD1); // PD1 auf LOW => ir-led an
OCR2 = 254-distance; // wenn OCR2=0xFE dann Objekt sehr nahe
z=0;
for(j=0;j<30;j++) // loop time: 5ms
{
if (PIND & (1 << PD0))z++;
Sleep(6); // 6*Sleep(6)=1ms
}
PORTD |= (1 << PD1); // PD1 auf High led auschalten
Msleep(1); // kurz warten
if (z>=29) return FALSE; // Objekt nicht gefunden
else return TRUE;
}
//-------------------------------------------------------------------
//-------------------------------------------------------------------
//hauptprogramm
int main(void)
{
Init();
objekt_weit = objekt_sichtbar(weit);
while(1)
{
objekt_weit = objekt_sichtbar(weit);
//-------------------------------------------------------------------
//wenn objekt gesehen, verfolgen!!
if (objekt_weit == 1)
{
StatusLED(YELLOW);
BackLEDleft(OFF);
BackLEDright(OFF);
MotorDir(FWD,FWD);
richtung = 0;
while ((objekt_sichtbar(weit) == 1) && (objekt_sichtbar(nah) == 0))
{
//fahr nach links
if ((objekt_sichtbar(weit) == 1) && (richtung == 0))
{
i=0;
while (i<= schwenkdauer)
{
StatusLED(YELLOW);
BackLEDleft(HALF);
BackLEDright(OFF);
MotorSpeed(100,155);
richtung=1;
i++;
Msleep(1);
}
i=0;
}
//fahr nach rechts
if ((objekt_sichtbar(weit) == 1) && (richtung == 1))
{
i=0;
while (i<=schwenkdauer)
{
StatusLED(YELLOW);
BackLEDleft(OFF);
BackLEDright(HALF);
MotorSpeed(155,100);
richtung=0;
i++;
Msleep(1);
}
i=0;
}
}
//wenn kein objekt mehr zu sehen ist
if (objekt_sichtbar(weit) == 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
BackLEDleft(OFF);
BackLEDright(OFF);
if (richtung == 0)
{
i = 0;
while ((objekt_sichtbar(weit) == 0) && (i<=verlierdauer))
{
StatusLED(RED);
BackLEDleft(HALF);
BackLEDright(OFF);
MotorSpeed(0,255);
richtung=0; //danach mit links anfangen
Msleep(1);
i++;
}
}
//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) && (i<=verlierdauer))
{
StatusLED(RED);
BackLEDleft(OFF);
BackLEDright(HALF);
MotorSpeed(255,0);
richtung=1; //danach mit rechts anfangen
Msleep(1);
i++;
}
StatusLED(OFF);
BackLEDleft(OFF);
BackLEDright(OFF);
}
}
//wenn objekt ganz nah, stehen bleiben
if (objekt_sichtbar(nah) == 1)
{
StatusLED(RED);
BackLEDleft(ON);
BackLEDright(ON);
MotorDir(FWD,FWD);
MotorSpeed(0,0); //stehen bleiben
Msleep(1);
}
}
//-------------------------------------------------------------------
}
while (1);
return 0;
}
Ausprobieren erlaubt, Anregung und Kritik erwünscht.
Zitat von robo.fr
Berechtigungen
Zitieren
Lesezeichen