Code:
#include <asuro.h>
#include <string.h> // für strlen
/*Left min/max, right min/max values da minimiert auf acht bit *4
+00150 *4 = 600 //data[1] left
+00222 *4 = 888
+00190 *4 = 760 //data[0] right
+00236 *4 = 944
*/
//Variablenverageb
int speed; //PWM des Rades
float msleft; //Metre pro Sekunde (Radgeschwindigkeit links
float msright; //Metre pro Sekunde (Radgeschwindigkeit rechts
float msleft1; // Vorherige Geschwindigkeit (für a)
float msright1; // Vorherige Geschwindigkeit (für a)
float mleft; //Gefahrene Strecke links
float mright; //Gefahrene Strecke rechts
float aleft; //Beschleunigung linkes Rad
float aright; //Beschleunigung rechtes Rad
unsigned int data [2]; //Array erstellen mit 2 Speicherzellen für Odometriedaten
//data [0] links T11
//data [1] rechts T12
int Zeit = 1000; //Zeit in der die Impulse gezählt werden 1s
int Zeit1 = 0; //letzte Zeit zur bestimmung der Zeit dif (für a)
int triggerleft = (((222-150)/2)+150)*4; //Helldunkelwechsel linkes Rad
int leftimpuls = 0; //gezählte Impulse pro Zeit links
int triggerright = (((236-190)/2)+190)*4; //Helldunkelwechsel rechtes Rad
int rightimpuls = 0; //gezählte Impulse pro Zeit rechts
unsigned int long stopzeit;
float pie = 3.14; //15927; //pie
float Umfang = 12; //.0165920775; //(38.25*pie); //Umfang in cm
int Muster = 12; //anzahl der Hell Dunkel übergänge
//-----------------------------------------------------------------------------
int main(void)
{
BackLED(OFF,OFF); //alle LED werden im Programm zur Veranschaulichung genutzt
StatusLED(RED); //d.h. wo bin ich im Prog. die Backeled´s sind aus da Odometrie an
Init();
while(1){
//-----------------------------------------------------------------------------
//Intro für Hyperterminal
//-----------------------------------------------------------------------------
SerPrint("\t Telemetriedaten\n\r\n\r");
//Überschrift im Hyperterminal für Vorwärts
SerPrint("\tVorwärts\n\r\n\r");
//-----------------------------------------------------------------------------
//Los gehts beschleunigen
//-----------------------------------------------------------------------------
StatusLED(GREEN);
MotorDir(FWD,FWD); //Richtung festlegen
Zeit1 = 0 ; //muss vor der Schleife genullt werden fals nioch restwert aus letzter Schleife
msright1 = 0;
msleft1 = 0;
for (speed=80;speed<=255;speed +=10){ //Geschwindigkeit von 80 auf 255 in 10er schritten
MotorSpeed(speed,speed); //Geschwindigkeit einlesen
leftimpuls = 0; //Impullse vor Zählvorgang auf null
rightimpuls = 0; //Impullse vor Zählvorgang auf null
//Detektierung der Impulse
stopzeit=Gettime()+Zeit; //1 sekunde Hell Dunkel Wechsel detektieren
do {
OdometrieData(data); //Odo daten bereitstellen
if(data[0]==triggerright) //wenn data = trigerlevel
rightimpuls++; //impulse erhöhen
if(data[1]==triggerleft) //siehe zwei drüber
leftimpuls++; //siehe zwei drüber
} while (stopzeit>Gettime()); //das ganze bis Zeit um (1s)
//Auswertung der Impulse
//Geschwindigkeit
msright = (rightimpuls * ((Umfang*pie)/Muster))/(Zeit*1000); //da ms
msleft = (leftimpuls * ((Umfang*pie)/Muster))/(Zeit*1000); //da ms
//Zurückgelegter Weg
mright = msright * Zeit;
mleft = msleft * Zeit;
//Beschleunigung
aright = (msright-msright1) / (Zeit-Zeit1);
aleft = (msleft-msleft1) / (Zeit-Zeit1);
Zeit1 = Zeit; //jetzige Zeit Zwischenspeichern für a beim nächsen turn
msright1 = msright; //jetzige Geschwindigkeit Zwischenspeichern für a beim nächsen turn
msleft1 = msleft; //jetzige Geschwindigkeit Zwischenspeichern für a beim nächsen turn
//Ausgabe
//rechtes
SerPrint("Rechtes Rad ");
PrintFloat(mright,2,2);
SerPrint (" m ");
PrintFloat(msright,2,2);
SerPrint(" cm/s" );
PrintFloat(aright,2,2);
SerPrint(" cm/s²");
//Lehrstellen zwischen rechts und links
SerPrint(" ");
//links
SerPrint("Linkes Rad ");
PrintFloat(mleft,2,2);
SerPrint (" m ");
PrintFloat(msleft,2,2);
SerPrint(" cm/s ");
PrintFloat(aleft,2,2);
SerPrint(" cm/s²");
//2*absatz
SerPrint("\n\r\n\r");
}
//-----------------------------------------------------------------------------
//Langsamer werden
//-----------------------------------------------------------------------------
Zeit1 = 0 ;
msright1 =0;
msleft1 =0;
for (speed=255;speed>=80;speed -=10){
MotorSpeed(speed,speed);
leftimpuls = 0;
rightimpuls = 0;
//Detektierung der Impulse
stopzeit=Gettime()+Zeit; //1 sekunde Hell Dunkel Wechsel detektieren
do {
OdometrieData(data);
if(data[0]==triggerright)
rightimpuls--;
if(data[1]==triggerleft)
leftimpuls--;
} while (stopzeit>Gettime());
//Auswertung der Impulse
//Geschwindigkeit
msright = (rightimpuls * ((Umfang*pie)/Muster))/(Zeit*1000); //da ms
msleft = (leftimpuls * ((Umfang*pie)/Muster))/(Zeit*1000); //da ms
//Zurückgelegter Weg
mright = msright * Zeit;
mleft = msleft * Zeit;
//Beschleunigung
aright = (msright-msright1) / (Zeit-Zeit1);
aleft = (msleft-msleft1) / (Zeit-Zeit1);
Zeit1 = Zeit;
msright1 = msright;
msleft1 = msleft;
//Ausgabe
//rechtes
SerPrint("Rechtes Rad ");
PrintFloat(mright,2,2);
SerPrint (" m ");
PrintFloat(msright,2,2);
SerPrint(" cm/s" );
PrintFloat(aright,2,2);
SerPrint(" cm/s²");
//Lehrstellen zwischen rechts und links
SerPrint(" ");
//links
SerPrint("Linkes Rad ");
PrintFloat(mleft,2,2);
SerPrint (" m ");
PrintFloat(msleft,2,2);
SerPrint(" cm/s ");
PrintFloat(aleft,2,2);
SerPrint(" cm/s²");
//2*absatz
SerPrint("\n\r\n\r");
}
//-----------------------------------------------------------------------------
//Rückwärts
//-----------------------------------------------------------------------------
//Überschrieft im Hyperterminal
SerPrint("\t Rückwärts\n\r\n\r");
//-----------------------------------------------------------------------------
//Los gehts beschleunigen
//-----------------------------------------------------------------------------
StatusLED(YELLOW);
MotorDir(RWD,RWD);
Zeit1 = 0 ;
msright1 =0;
msleft1 =0;
for (speed=80;speed<=255;speed +=10){
MotorSpeed(speed,speed);
leftimpuls = 0;
rightimpuls = 0;
//Detektierung der Impulse
stopzeit=Gettime()+Zeit; //1 sekunde Hell Dunkel Wechsel detektieren
do {
OdometrieData(data);
if(data[0]==triggerright)
rightimpuls++;
if(data[1]==triggerleft)
leftimpuls++;
} while (stopzeit>Gettime());
//Auswertung der Impulse
//Geschwindigkeit
msright = (rightimpuls * ((Umfang*pie)/Muster))/(Zeit*1000); //da ms
msleft = (leftimpuls * ((Umfang*pie)/Muster))/(Zeit*1000); //da ms
//Zurückgelegter Weg
mright = msright * Zeit;
mleft = msleft * Zeit;
//Beschleunigung
aright = (msright-msright1) / (Zeit-Zeit1);
aleft = (msleft-msleft1) / (Zeit-Zeit1);
Zeit1 = Zeit;
msright1 = msright;
msleft1 = msleft;
//Ausgabe
//rechtes
SerPrint("Rechtes Rad ");
PrintFloat(mright,2,2);
SerPrint (" m ");
PrintFloat(msright,2,2);
SerPrint(" cm/s" );
PrintFloat(aright,2,2);
SerPrint(" cm/s²");
//Lehrstellen zwischen rechts und links
SerPrint(" ");
//links
SerPrint("Linkes Rad ");
PrintFloat(mleft,2,2);
SerPrint (" m ");
PrintFloat(msleft,2,2);
SerPrint(" cm/s ");
PrintFloat(aleft,2,2);
SerPrint(" cm/s²");
//2*absatz
SerPrint("\n\r\n\r");
}
//-----------------------------------------------------------------------------
//Langsamer werden
//-----------------------------------------------------------------------------
Zeit1 = 0 ;
msright1 =0;
msleft1 =0;
for (speed=255;speed>=80;speed -=10){
MotorSpeed(speed,speed);
leftimpuls = 0;
rightimpuls = 0;
//Detektierung der Impulse
stopzeit=Gettime()+Zeit; //1 sekunde Hell Dunkel Wechsel detektieren
do {
OdometrieData(data);
if(data[0]==triggerright)
rightimpuls--;
if(data[1]==triggerleft)
leftimpuls--;
} while (stopzeit>Gettime());
//Auswertung der Impulse
//Geschwindigkeit
msright = (rightimpuls * ((Umfang*pie)/Muster))/(Zeit*1000); //da ms
msleft = (leftimpuls * ((Umfang*pie)/Muster))/(Zeit*1000); //da ms
//Zurückgelegter Weg
mright = msright * Zeit;
mleft = msleft * Zeit;
//Beschleunigung
aright = (msright-msright1) / (Zeit-Zeit1);
aleft = (msleft-msleft1) / (Zeit-Zeit1);
Zeit1 = Zeit;
msright1 = msright;
msleft1 = msleft;
//Ausgabe
//rechtes
SerPrint("Rechtes Rad ");
PrintFloat(mright,2,2);
SerPrint (" m ");
PrintFloat(msright,2,2);
SerPrint(" cm/s" );
PrintFloat(aright,2,2);
SerPrint(" cm/s²");
//Lehrstellen zwischen rechts und links
SerPrint(" ");
//links
SerPrint("Linkes Rad ");
PrintFloat(mleft,2,2);
SerPrint (" m ");
PrintFloat(msleft,2,2);
SerPrint(" cm/s ");
PrintFloat(aleft,2,2);
SerPrint(" cm/s²");
//2*absatz
SerPrint("\n\r\n\r");
}
}
return 0;}
Viel spaß und schon mal danke
Lesezeichen