Code:
//-------------------------------------------------------------------
//-------------------------------------------------------------------
//
// joghurt-sumo-wettkampf
// M1.R
// joghurt_sumov02
//
//-------------------------------------------------------------------
//-------------------------------------------------------------------
#include "asuro.h"
#define HALF 2
//-------------------------------------------------------------------
//variable
uint16_t hell_links, hell_rechts, i, zuf;
uint8_t linie, objekt_weit, objekt_nah, rechts, speed1, speed2, richtung, abbrechen;
//-------------------------------------------------------------------
// defines anpassen !!!!!!!!!!!!!!!!!!!!!!!!!!
//schwellenwert linie mit heller led
//weiss:links 600-650, rechts 500-550
//schwarz: links 25-30, rechts 32
#define achtung_linie 200 //200
//abstand für ir-messung
#define nah 1 //1
#define weit 14 //max 16
#define geradeaus 200 ////*6ms dauer geradeausfahrt 200 entspricht ca. 80cm
#define leichtekurve 200 ////*6ms dauer geradeausfahrt 200 entspricht ca. 80cm
#define schwenkdauer 60
#define verlierdauer 500
#define rauszeit 4000
#define rueckwaertszeit 500
#define liniendauer1 500
#define liniendauer2 500
#define liniendauer3 500
#define wendedauer 100
//-------------------------------------------------------------------
//projekt-funktionen
//-------------------------------------------------------------------
void BackLEDleft(uint8_t status);
void BackLEDright(uint8_t status);
void akku_kontrolle (void);
uint8_t zufallbat(void);
uint8_t objekt_sichtbar(uint8_t abstand);
//-------------------------------------------------------------------
//-------------------------------------------------------------------
//akku kontrolle
//berechnung: wert=spannung/0.0055
//1090 entsprechen ca. 6 volt
//909 entsprechen ca. 5 volt
//810 entsprechen ca. 4,455 Volt
//wenn kleiner als 4,455 volt rot blinken motoren aus
//wenn kleine als 5 volt 3 sec gelb blinken
//wenn >= 5 volt led grün 3 sec
void akku_kontrolle (void)
{
uint16_t wert;
uint8_t i;
StatusLED(OFF);
Msleep(1000);
wert = Battery();
if (wert < 810)
{
for(i = 1; i <= 10; ++i)
{
StatusLED(RED);
Msleep(100);
StatusLED(OFF);
Msleep(50);
}
}
if (wert < 909)
{
for(i = 1; i <= 10; ++i)
{
StatusLED(YELLOW);
Msleep(100);
StatusLED(OFF);
Msleep(50);
}
}
else
{
for(i = 1; i <= 10; ++i)
{
StatusLED(GREEN);
Msleep(100);
StatusLED(OFF);
Msleep(50);
}
}
}
//-------------------------------------------------------------------
//------------------------------------------------------------------
//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;
}
//-------------------------------------------------------------------
// liniensuche
// 1 wenn getroffen, 0 wenn nicht
uint8_t linie_getroffen(uint16_t schwellenwert)
{
FrontLED(ON);
uint16_t data[2];
LineData(data);
hell_links = data[0];
hell_rechts = data[1];
if ((hell_links >= achtung_linie) && (hell_rechts >= achtung_linie))
return FALSE; // keine linie
else return TRUE;
}
//-------------------------------------------------------------------
// linie getroffen
void linie_ueberfahren (void)
{
i = 0;
StatusLED(OFF);
BackLEDleft(OFF);
BackLEDright(OFF);
//ein kleines stückchen vorwärts
MotorDir(FWD,FWD);
MotorSpeed(255,255);
Msleep(30);
//wenn die linie noch nicht überfahren ist, noch weiter vorwärts
while((linie_getroffen(achtung_linie) == 1) && (i<= liniendauer1))
{
MotorDir(FWD,FWD);
MotorSpeed(255,255);
Msleep(1);
i++;
}
i = 0;
//jenseits der linie vollbremsung!
MotorDir(BREAK,BREAK);
StatusLED(RED);
Msleep(100);
StatusLED(OFF);
//rückwärts bis linie getroffen
while((linie_getroffen(achtung_linie) == 0) && (i<= liniendauer2))
{
MotorDir(RWD,RWD);
MotorSpeed(255,255);
Msleep(1);
i++;
}
//wenns rechts heller ist, ist die linie rechts
if(hell_rechts>=hell_links)
{
rechts = 1;
BackLEDleft(OFF);
BackLEDright(HALF);
}
if(hell_links>hell_rechts)
{
rechts = 0;
BackLEDleft(HALF);
BackLEDright(OFF);
}
i = 0;
//rückwärts bis keine linie mehr
//mit kurve
while((linie_getroffen(achtung_linie) == 1) && (i<= liniendauer3))
{
if (rechts == 1) //wenn die linie rechts ist
{
MotorDir(RWD,RWD);
MotorSpeed(80,255);
}
else if (rechts == 0) //wenn die linie links ist
{
MotorDir(RWD,RWD);
MotorSpeed(255,80);
}
Msleep(1);
i++;
}
//noch ein stückchen rückwärts
MotorDir(RWD,RWD);
MotorSpeed(255,255);
Msleep(20);
//vollbremsung
MotorDir(BREAK,BREAK);
Msleep(100);
//und jetzt wenden
i=0;
while ((linie_getroffen(achtung_linie)==0) && (i<wendedauer)) //i<?? => 180 grad wendung
{
if(rechts == 1)
{
MotorDir(RWD,FWD); //rechts dunkler also ist die linie rechts dreh nach links
}
else if(rechts == 0)
{
MotorDir(FWD,RWD);
}
MotorSpeed(255,255);
Msleep(1);
i++;
}
// anschliessend ein stück geradeaus, um wieder in die mitte zu kommen
// !!!! z einstellen!!!
i = 0;
while ((linie_getroffen(achtung_linie)==0) && (objekt_sichtbar(weit)==0) && (i<geradeaus))
{
MotorDir(FWD,FWD);
MotorSpeed(255,255);
BackLEDleft(OFF);
BackLEDright(OFF);
StatusLED(GREEN);
i++;
Msleep(1);
}
}
//------------------------------------------------------------------
// zufallbat()
// Pseudozufallsfunktion von robo.fr
// erweitert mit Batterie-Abfrage M1.R
// uint8_t zufallbat()
// Liefert eine 8Bit Zufallszahl zurück,
// int Batterie (void) in der Datei adc.c
// liefert 10-Bit-Wert der Batteriespannung (Bereich 0..1023)
//------------------------------------------------------------------
uint8_t zufallbat(void)
{
static uint16_t startwert=0x0AA;
uint16_t temp;
uint8_t n;
for(n=1;n<8;n++)
{
temp = startwert;
startwert=startwert << 1;
temp ^= startwert;
if ( ( temp & 0x4000 ) == 0x4000 )
{
startwert |= 1;
}
}
startwert^= Batterie(); // macht den Pseudozufall wirklich zufällig
return (startwert);
}
//------------------------------------------------------------------
//-------------------------------------------------------------------
// linie getroffen
void linie_wenden (void)
{
StatusLED(RED);
MotorDir(RWD,RWD);
MotorSpeed(255,255);
Msleep(200);
//rechts dunkler also ist die linie rechts dreh nach links
if(hell_rechts <= hell_links)
{
i=0;
while ((linie_getroffen(achtung_linie)==0) && (i<wendedauer)) //i<?? => 180 grad wendung
{
MotorDir(RWD,FWD);
BackLEDleft(OFF);
BackLEDright(HALF);
MotorSpeed(255,255);
Msleep(1);
i++;
}
// anschliessend ein stückchen leichte kurve, um wieder in die mitte zu kommen
i = 0;
while ((linie_getroffen(achtung_linie)==0) && (objekt_sichtbar(weit)==0) && (i<leichtekurve))
{
StatusLED(OFF);
MotorDir(FWD,FWD);
MotorSpeed(200,255);
Msleep(1);
i++;
}
}
//links dunkler also ist die linie links dreh nach rechts
if (hell_links < hell_rechts)
{
i=0;
while ((linie_getroffen(achtung_linie)==0) && (i<wendedauer)) //i<?? => 180 grad wendung
{
MotorDir(FWD,RWD);
BackLEDleft(HALF);
BackLEDright(OFF);
MotorSpeed(255,255);
Msleep(1);
i++;
}
// anschliessend ein stückchen leichte kurve, um wieder in die mitte zu kommen
i = 0;
while ((linie_getroffen(achtung_linie)==0) && (objekt_sichtbar(weit)==0) && (i<leichtekurve))
{
StatusLED(OFF);
MotorDir(FWD,FWD);
MotorSpeed(255,200);
Msleep(1);
i++;
}
}
}
//-------------------------------------------------------------------
//-------------------------------------------------------------------
// linie getroffen beim rückwärtsfahren
void linie_vorwaerts (void)
{
StatusLED(RED);
while (linie_getroffen(achtung_linie)==1)
{
MotorDir(FWD,FWD);
MotorSpeed(255,255);
}
}
//-------------------------------------------------------------------
//-------------------------------------------------------------------
// linie getroffen beim rückwärtsfahren
void rueckwaerts (void)
{
StatusLED(RED);
i = 0;
while ((abbrechen == 0) && (i<= rueckwaertszeit))
{
linie = linie_getroffen(achtung_linie);
if (linie == 1)
{
abbrechen = 1;
linie_vorwaerts();
}
MotorDir(RWD,RWD);
MotorSpeed(255,255);
Msleep(1);
i++;
}
}
//-------------------------------------------------------------------
//-------------------------------------------------------------------
//-------------------------------------------------------------------
//hauptprogramm
int main(void)
{
Init();
//!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
//-------------------------------------------------------------------
//ab hier test
StatusLED(OFF);
Msleep(1000);
//akku_kontrolle();
//while(1);
//bis hier test !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
//-------------------------------------------------------------------
//-------------------------------------------------------------------
//-------------------------------------------------------------------
// letzte vorbereitungen
objekt_weit = objekt_sichtbar(weit);
linie = linie_getroffen(achtung_linie);
// hauptschleife
while(1)
{
objekt_weit = objekt_sichtbar(weit);
linie = linie_getroffen(achtung_linie);
//-------------------------------------------------------------------
//wenn linie getroffen...
if (linie == 1)
{
linie_wenden();
}
//-------------------------------------------------------------------
//wenn objekt gesehen, verfolgen!!
else if (objekt_weit == 1)
{
StatusLED(YELLOW);
BackLEDleft(OFF);
BackLEDright(OFF);
MotorDir(FWD,FWD);
richtung = 0;
abbrechen = 0;
linie = linie_getroffen(achtung_linie);
if (linie == 1)
{
abbrechen = 1;
linie_wenden();
}
while(abbrechen == 0)
{
while ((objekt_sichtbar(weit) == 1) && (abbrechen == 0) && (objekt_sichtbar(nah) == 0))
{
linie = linie_getroffen(achtung_linie);
if (linie == 1)
{
abbrechen = 1;
linie_wenden();
}
//fahr nach links
if ((objekt_sichtbar(weit) == 1) && (richtung == 0) && (abbrechen == 0))
{
i=0;
while ((abbrechen == 0) && (i<= schwenkdauer))
{
linie = linie_getroffen(achtung_linie);
if (linie == 1)
{
abbrechen = 1;
linie_wenden();
}
StatusLED(YELLOW);
BackLEDleft(HALF);
BackLEDright(OFF);
MotorSpeed(150,255);
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))
{
linie = linie_getroffen(achtung_linie);
if (linie == 1)
{
abbrechen = 1;
linie_wenden();
}
StatusLED(YELLOW);
BackLEDleft(OFF);
BackLEDright(HALF);
MotorSpeed(255,150);
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
BackLEDleft(OFF);
BackLEDright(OFF);
if (richtung == 0)
{
i = 0;
while ((objekt_sichtbar(weit) == 0) && (abbrechen == 0) && (i<=verlierdauer))
{
linie = linie_getroffen(achtung_linie);
if (linie == 1)
{
abbrechen = 1;
linie_wenden();
}
StatusLED(RED);
BackLEDleft(HALF);
BackLEDright(OFF);
MotorSpeed(150,255);
richtung=0; //danach mit links anfangen
Msleep(1);
i++;
}
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) && (i<=verlierdauer))
{
linie = linie_getroffen(achtung_linie);
if (linie == 1)
{
abbrechen = 1;
linie_wenden();
}
StatusLED(RED);
BackLEDleft(OFF);
BackLEDright(HALF);
MotorSpeed(255,150);
richtung=1; //danach mit rechts anfangen
Msleep(1);
i++;
}
abbrechen = 1;
StatusLED(OFF);
BackLEDleft(OFF);
BackLEDright(OFF);
}
}
//wenn objekt ganz nah, dann raus damit
if (objekt_sichtbar(nah) == 1)
{
StatusLED(RED);
BackLEDleft(ON);
BackLEDright(ON);
while ((abbrechen == 0) && (i<= rauszeit))
{
linie = linie_getroffen(achtung_linie);
if (linie == 1)
{
linie_ueberfahren();
abbrechen = 1;
}
MotorDir(FWD,FWD);
MotorSpeed(255,255); //schnell vorwärts!
Msleep(1);
i++;
if (i > rauszeit)
{
rueckwaerts();
abbrechen = 1;
}
}
}
}
}
//-------------------------------------------------------------------
//ansonsten rumfahren und objekt suchen
else
{
StatusLED(GREEN);
BackLEDleft(OFF);
BackLEDright(OFF);
MotorDir(FWD,FWD);
zuf = zufallbat();
zuf = zuf/128;
if (zuf == 0)
{
speed1 = 255;
zuf = zufallbat();
zuf = zuf/2;
speed2 = 100 + zuf;
BackLEDleft(HALF);
BackLEDright(OFF);
}
if (zuf == 1)
{
speed1 = 255;
zuf = zufallbat();
zuf = zuf/2;
speed2 = 100 + zuf;
BackLEDleft(OFF);
BackLEDright(HALF);
}
i=0;
abbrechen = 0;
while ((abbrechen == 0)&&(objekt_sichtbar(weit)==0) && (i<500))
{
linie = linie_getroffen(achtung_linie);
if (linie == 1)
{
linie_wenden();
abbrechen = 1;
}
MotorSpeed(speed1,speed2);
i++;
}
i=0;
abbrechen = 0;
while ((abbrechen == 0)&&(objekt_sichtbar(weit)==0) && (i<500))
{
linie = linie_getroffen(achtung_linie);
if (linie == 1)
{
linie_wenden();
abbrechen = 1;
}
MotorSpeed(speed2,speed1);
i++;
}
}
}
while (1);
return 0;
}
Zitat von pinsel120866
Zitieren
Lesezeichen