Hallo
Da mein asuro nicht fahrbereit ist, und mein probot-asuro keine Kollisitionstaster besitzt, kann ich das überhaupt nicht testen:
PHP-Code:
// Linie folgen und bei Hinderniss ausweichen 11.6.2012 mic
// asuro weicht abhängig von den Kollisionstastern links oder rechts aus und
// fährt einen Bogen um das Hindreniss während er die Linie sucht.
// Bei einem frontalen Hinderniss ist der Radius des Ausweichbogen größer.
// https://www.roboternetz.de/community/threads/58073-Linie-verfolgen-plus-Gegenst%C3%A4nden-ausweichen-plus-Linie-wiederfinden-Hilfe
#include "asuro.h" /*Linienverfolgung*/
#define tol 15 /*Toleranz der Sensoren*/
#define FULL_L 120 /* Fullspeed linker Motot */
#define FULL_R 120 /* Fullspeed rechter Motor */
/* Motor vorwärts */
void MotorFwd(void)
{
MotorDir(FWD,FWD);
MotorSpeed(FULL_L,FULL_R);
}
/* Motor rückwärts */
void MotorRwd(void)
{
MotorDir(RWD,RWD);
MotorSpeed(FULL_L,FULL_R);
}
void Msleep(int dauer)
{
int z;
for(z=0;z<dauer;z++) Sleep(72);
}
int main(void)
{
unsigned int data[2], keine_linie;
unsigned char t1, t2, ufs; // UmFahrSeite 0=links, 1=rechts, 2=links weit, 3=rechts weit
Init();
FrontLED(ON); // etwas verzögern weil a: die LED nicht schlagartig hell wird ...
LineData(data); // und b: der ADC bei der ersten Lesung gelegentlich spinnt
MotorDir(FWD,FWD);
while(1)
{
do // Linie folgen bis Hinderniss erkannt wird
{
LineData(data);
if (data [0] > data [1] + tol ) // Linie schwenkt nach rechts
{
MotorSpeed(FULL_L+20,FULL_R-40);
MotorDir(FWD,RWD);
}
else if (data [1] > data [0] + tol ) // Linie schwenkt nach links
{
MotorSpeed(FULL_L-40,FULL_R+20);
MotorDir(RWD,FWD);
}
else // auf der Linie (immer inclusive tol)
{
MotorSpeed(FULL_L,FULL_R);
MotorDir(FWD,FWD);
}
t1 = PollSwitch();
t2 = PollSwitch();
}while((t1!=t2) || (t1==0)); // nur wenn t1 gleich t2 ist der Tastenwert gülig
StatusLED(RED);
MotorDir(BREAK,BREAK); // Notstopp
MotorSpeed(0,0);
Msleep(500); // durchatmen
if((t1 & 0b000111) && !(t1 & 0b111000)) // k4 bis k6 Hinderniss auf der rechten Seite
{
ufs=0;
MotorDir(RWD,FWD); // 90° nach links schwenken
MotorSpeed(FULL_L,FULL_R);
Msleep(500); // Zeit muss angepasst werden für ca. 90°
}
else if((t1 & 0b111000) && !(t1 & 0b000111)) // k1 bis k3 Hinderniss auf der linken Seite
{
ufs=1;
MotorDir(FWD,RWD); // 90° nach rechts schwenken
MotorSpeed(FULL_L,FULL_R);
Msleep(500); // Zeit muss angepasst werden für ca. 90°
}
else
{
if(data[0] & 1) // 90° nach Zufall schwenken
{
ufs=2; // großer Bogen nach links
MotorDir(RWD,FWD);
}
else
{
ufs=3; // großer Bogen nach rechts
MotorDir(FWD,RWD);
}
MotorSpeed(FULL_L,FULL_R);
Msleep(500); // Zeit muss angepasst werden für ca. 90°
}
MotorSpeed(0,0); // anhalten
Msleep(500);
LineData(data); // Aktuelle Helligkeit messen da hier wohl keine Linie ist
keine_linie=data[0]+data[1];
LineData(data); // Durchschnitt von zwei Messungen
keine_linie=(keine_linie+data[0]+data[1])/2;
keine_linie=keine_linie-tol*2; // irgendwie den Schwellwert für die Linie berechnen
StatusLED(YELLOW);
MotorDir(FWD,FWD); // Umfahrbogen festlegen und Linie suchen
if(ufs & 1) // bit 0 ist links oder rechtskurve
if(ufs & 2) // bit 1 gesetzt bedeutet weite Kurve (ufs ist 2 oder 3)
MotorDir(FULL_L-20,FULL_R); // rechts ausweichen, Linksbogen fahren weit
else
MotorDir(FULL_L-30,FULL_R); // rechts ausweichen, Linksbogen fahren eng
else
if(ufs & 2)
MotorDir(FULL_L,FULL_R-20); // links ausweichen, Rechtsbogen fahren weit
else
MotorDir(FULL_L,FULL_R-30); // links ausweichen, Rechtsbogen fahren eng
do // Kurve fahren und dabei die Linie suchen
LineData(data);
while(keine_linie > (data[0]+data[1])); // solange die Linienwerte nicht klein genug sind
StatusLED(GREEN); // Linie gefunden, folgen
MotorSpeed(FULL_L,FULL_R);
}
return 0;
}
(ungetestet)
Wenn davon irgendetwas funktioniert, wäre es nett, wenn du ein Filmchen machen würdest.
Genauere Angaben zur Linie, dem Kurs und dem Hinderniss wären echt hilfreich.
Gruß
mic
Linie verfolgen plus Gegenständen ausweichen plus Linie wiederfinden Hilfe
Zitieren
Lesezeichen