Hab nun noch ein bisschen herumprobiert und hab mir schließlich noch den Zusammenhang zwischen MotorSpeed Änderung und der Geschwindigkeit (gemessen anhand der Odometrie) angesehen.
Interessanterweise gibt Asuro bei einer Odometrieabtastzeit von weniger als 1ms falsche Werte zurück, siehe Bildanhang, blaue Linie. Je höher die Motorgeschwindigkeit, desto geringer die gemessene Geschwindigkeit!!! Da ist was faul. Anscheinend ist die Odometriefunktion bei so hoher Abtastrate unbrauchbar!? Wie auch immer...
Bei einer Abtastzeit von 2ms(rote Linie) hat man einen fast linearen Zusammenhang zwischen MotorSpeed und gemessener Geschwindigkeit.
Mit den neuen Erkenntnissen habe ich mein Programm überarbeitet und siehe da - Asuro fährt fast immer geradeaus. Das ganze funktioniert quasi wie ein P-Regler.
Code:
/*
Erste Experimente mit Asuro.
3.7.2007
von harry3
*/
#define MAX_DIFF 0
#define MSTART_L 140
#define MSTART_R 140
#define ZEITKONST 250
#define ABTASTZEIT 2
#define KP 3
#define KORR_MAX 36
#define TASTER(x) (unsigned char)(1<<(x-1))
#include "asuro.h"
void odometrie_auswertung(int,int*,int*);
int main(void)
{
unsigned char mspeed_l=MSTART_L,mspeed_r=MSTART_R,temp=0;
unsigned int array_l[3]={0},array_r[3]={0},odata[2]={0},timer=0;
signed int counter_l=0,counter_r=0,korr=0;
Init();
while(1)
{
temp=PollSwitch();
if(temp==PollSwitch() && temp!=0)
break;
else
{
MotorDir(FWD,FWD);
MotorSpeed(mspeed_l,mspeed_r);
OdometrieData(odata);
odometrie_auswertung(odata[0],array_l,&counter_l);
odometrie_auswertung(odata[1],array_r,&counter_r);
if(timer>ZEITKONST)
{
if( (counter_l-counter_r) > MAX_DIFF)
{
korr=(counter_l-counter_r)*KP;
if(korr>KORR_MAX)
korr=KORR_MAX;
if(mspeed_r<=(255-korr))
mspeed_r=mspeed_r+korr;
}
else if( (counter_r-counter_l) > MAX_DIFF)
{
korr=(counter_r-counter_l)*KP;
if(korr>KORR_MAX)
korr=KORR_MAX;
if(mspeed_r>=korr)
mspeed_r=mspeed_r-korr;
}
counter_l=0;
counter_r=0;
timer=0;
}
timer++;
Msleep(ABTASTZEIT);
}
}
MotorDir(BREAK,BREAK);
//In Endlosschleife verbleiben
while(1);
return 0;
}
void odometrie_auswertung(int eingabe,int* zwsp,int* counter)
{
zwsp[2]=zwsp[1];
zwsp[1]=zwsp[0];
zwsp[0]=eingabe;
if(zwsp[1]>zwsp[0] && zwsp[1]>zwsp[2])
(*counter)++;
else if(zwsp[1]<zwsp[0] && zwsp[1]<zwsp[2])
(*counter)++;
return;
}
Grüße,
Harri
Zitieren
Lesezeichen