Hinderniserkennung über die Spannung
Hi,
ich habe mir letztens überlegt eine Hinderniserkennung für Asuro zu schreiben die blockierte Räder erkennt also Asuro unterschtützt wenn die Taster vorne überfordert sind mit der Situation.
Ich habe mal eine Funktion geschrieben:
Code:
int kollision()
{
unsigned int ergebnisse[9];
int ableitung[8];
for (int b = 0; b < 9; b++)
{
ADMUX |= (1 << REFS0) | (1 << REFS1) | (1 <<MUX0) | (1 << MUX2); // interne Referenz und ADC5( als analoger Kanal )
ADCSRA |= (1 << ADSC); // conversation START
while( !(ADCSRA & ( 1 << ADIF) ) ) // ADIF ist 0
{
; // Mache garnichts
}
// Messung ist zuende, da ADIF = 1
ADCSRA &= ~( 1 << ADIF ); // Setze ADIF auf 0
ergebnisse[b] = ADCL | ( ADCH << 8);
}
for (int c = 0; c<8;c++)
{
ableitung[c] = ergebnisse[c] - ergebnisse[c+1]; // Steigung zwischen den Messungen
}
for (int d = 0; d<8;d++)
{
if(ableitung[d] < (-25) )
{
if(d<7)
{
if((ergebnisse[d] - ergebnisse[d+2]) < (-10)) // Ableitung zum nächsten Punkt also Spitzen ausschließen
{
return TRUE;// Kollision
}
}
else // die letzte Ableitung also kein Vergleich nach vorne möglich
{
// Dann einen Messpunkt holen und vergleichen
int data;
// Messpunkt wird geholt
ADMUX |= (1 << REFS0) | (1 << REFS1) | (1 <<MUX0) | (1 << MUX2); // interne Referenz und ADC5( als analoger Kannal )
ADCSRA |= (1 << ADSC); // conversation START
while( (ADCSRA & ( 1 << ADIF) ) ) // ADIF ist noch 0
{
; // Mache nichts
}
// Messung ist zuende, da ADIF = 1
ADCSRA &= ~( 1 << ADIF ); // Setze ADIF auf 0
data = ADCL | ( ADCH << 8);
// Jetzt kann man bestimmen, ob es eine Fehlmessung war oder nicht
if((ergebnisse[d] - data) < (-10)) // Ableitung zum nächsten Punkt also Spitzen ausschließen
{
return TRUE;// Kollision
}
}//else
}//if
}//for
return FALSE;//keine Kollision
}
Also bei mir funktioniert sie, wenn man beide Räder festhält.
Damit Asuro umdreht habe ich eine Funktion zum umdrehen geschrieben.
Ich habe mich dort für die Odometrie entschieden, da die Messungen nicht wirklich genau sind und man somit eine art Zufall beim umdrehen erhält.
Code:
void umdrehen()
{
unsigned int odom[2];
unsigned int odomSumme = 0;
for ( int i= 0; odomSumme < 20000;i++)
{
MotorDir(RWD,RWD);
OCR1A = 230; // Setze Motor Speed auf 230 von 255
OCR1B = 150; // Setze Motor Speed auf 150 von 255
OdometrieData(odom);
odomSumme += odom[1]/100;
}
//wieder geradeaus
MotorDir(FWD,FWD);
OCR1A = 230; // Setze Motor Speed auf 230 von 255
OCR1B = 230; // Setze Motor Speed auf 250 von 255
PORTD &= ~(1 << PD2); // Status LED rot aus
for (int i= 0; i<500;i++)
{
if(kollision()==FALSE && i > 250) // Warten bis Motoren wieder in die richtige Richtung fahren
{
break;
}
}
}
Beim umdrehen achte ich darauf, dass die Motoren angelaufen sind und somit sichergestellt ist, dass nicht schon wieder eine Blockade der Räder festgestellt wird.
Mein Großes Problem dabei ist, dass Asuro nach höchstens 5 mal umdrehen Abstürzt.
Es fangen alle Lichter an zu Leuchten. Oder es dreht sich nur ein Rad. Oder Asuro fährt dauerhaft geradeaus und reagiert gar nicht mehr.
Irgendwo muss ich einen oder mehrer große(n) Fehler machen.
THX cucu
