Odometrie_Programm fehlerhaft?brauch dringend hilfe
Hallo an alle :)
Ich wollte fragen ob ihr vielleicht meinen code anschaun könnt. Das ganze funktioniert nicht ganz so wie ich will
Ich habe folgendes Problem:
wenn ich den Asuro einschalte und die Motoren sich drehen und mir die Zählerstände über das Hyperterminal zurückschreben lasse, dann seh ich das es mir nicht jeden Farbwechsel mitzählt, sondern nur jeden 20. oder 30. :(
Wenn ich die Motoren von den Zahnrädern wegnehme, und von Hand langsam drehe dann zählt er jeden Farbwechsel, aber sobald ich ein bisschen schneller drehe, überspringt er einige Farbwechsel.
Das Programm soll den Asuro eigentlich bei der Geradeausfahrt eigentlich Regeln. Die Regelung funktioniert auch, nur er zählt zu langsam.
Code:
#include "stdlib.h"
#include "asuro.h"
void SerWert(unsigned int iWert)
{
unsigned char cWert[6] = " ";
itoa(iWert, cWert, 10);
SerWrite(cWert, 5);
SerWrite(", ", 2);
}
int main(void)
{
/*Merker Links und REchts*/
unsigned int ML = 0;
unsigned int MR = 0;
/*Zähler für farbwechsel*/
unsigned int zaehlerl = 0;
unsigned int zaehlerr = 0;
/*geschwindigkeit der Motoren*/
int speed = 100; //Grundgeschwingkeit
int speedl = 0; //Geregelte Geschwindigkeit links
int speedr = 0; //Geregelte Geschwindigkeit Rechts
/* odometrie Datenspeicher */
unsigned int data[2]={0,0};
/*Schwellwerte*/
unsigned int SO = 500;
unsigned int SU = 200;
/*Differenz*/
int diff = 0;
/*Verstärkungsfaktor*/
int VFaktor = 2;
Init();
StatusLED(GREEN);
// Merker Links auf Anfangswert setzten
if (data[0] > SO)
{
ML=1;
}
if (data [0] < SU)
{
ML=2;
}
// Merker Rechts auf Anfangswert setzten
if (data [1] > SO)
{
MR=1;
}
if (data [1] < SU)
{
MR=2;
}
while(1) {
MotorDir(FWD,FWD);
MotorSpeed(speedl,speedr);
OdometrieData(data); // links = 0, rechts = 1
// Odometrie Auswertung Links
if ((data[0]) > SO && (ML==2)) // wenn Helligkeitswert links > Schwelle oben und Merker links ist 2
{
zaehlerl ++; ML=1; // dann Zähler +1 und setze Merker auf 1
}
if ((data[0]) < SU && (ML==1)) // wenn Helligkeitswert links < Schwelle unten und Merker links ist 1
{
zaehlerl ++; ML=2; // dann Zähler +1 und setze Merker auf 2
}
// Odometrie Auswertung Rechts
if ((data[1]) > SO && (MR==2)) // wenn Helligkeitswert rechts > Schwelle oben und Merker rechts ist 2
{
zaehlerr ++; MR=1; // dann Zähler +1 und setze Merker auf 1
}
if ((data[1]) < SU && (MR==1)) // wenn Helligkeitswert rechts < Schwelle unten und Merker rechts ist 1
{
zaehlerr ++; MR=2; // dann Zähler +1 und setze Merker auf 2
}
// Zälerdifferenz ermitteln
diff = zaehlerl - zaehlerr;
speedl = speed - VFaktor * diff;
speedr = speed + VFaktor * diff;
SerWert(zaehlerl);
SerWert(zaehlerr);
SerWert(ML);
SerWert(MR);
SerWert(data[0]);
SerWert(data[1]);
SerWrite("\n\r", 2);
}
return 0;
}
hat jemand eine Idee??
Vielen Dank im Vorraus
Gruß Schleicha
