Neuling: Odometrie

[teHIngo]

Zwischenbericht: Überarbeitet, klappt immer noch nicht. : (
Soll ich vielleicht einfach nochmal alles schön ordentlich kurz und präzise hinschreiben und einen neuen Thread eröffnen? Wirkt vielleicht nicht so abschreckend und es würde evtl. mal jemand antworten ; )

/* ####################################################### */
/* ############# FUNKTIONEN DEFINIEREN ################### */
/* ####################################################### */

#include "asuro.h"	// Konfigurationsdateien laden

#define MITTER 155	// ca. die Hälfte der Amplitude
#define MITTEL 140	// der Odometriewerte. Benötigt für
			// Initialisierung

/* ####################################################### */
/* ############# FUNKTIONEN DEFINIEREN ################### */
/* ####################################################### */

/* Globale Variablen */
unsigned char i;				// Zähler
char alr=0;					// schon (already) gelaufen?
char alr2=0;				// schon (already) gelaufen?
unsigned int cntsL=0;				// Zähler (counts)
unsigned int cntsR=0;				// Zähler
unsigned int cnts=0;				// Zähler
unsigned int odoData[2];			// Array für Odo Werte
unsigned int lrHi[2]={MITTEL, MITTER};		// left right highest
unsigned int lrLo[2]={MITTEL, MITTER};		// left right lowest
unsigned char spd0=190; 			// Geschwindigkeit (speed)
unsigned char spd1=190; 			// Geschwindigkeit
unsigned int odoMemL[3]={0,0,0};;  	    	// Ododaten zwischenspeichern (ododmetrie memory)
unsigned int odoMemR[3]={0,0,0};;   	   	// Ododaten zwischenspeichern
unsigned int testErg[2]={MITTEL, MITTER};	// Ergebnisse des Durchlaufes (Testergebnis)
                              			// 0=low, 1=hi

/* --- Verbesserte Sleep Funktion --- */
void schlafen(unsigned int wert){
 for(i=0; i<=wert; i++) Sleep(72);
}

/* - Geradeausfahren Initialisieren - */
char InitFwd(void){

   schlafen(250);
   StatusLED(RED);
   schlafen(250);
   StatusLED(YELLOW);

   MotorDir(FWD,FWD);   // Zunächst gasgeben damit er fährt und Werte sammelt
   MotorSpeed(190,190);
   schlafen(250);	// Zeit zum Beschleunigen geben

   /* Messung beginnen */
   while(cnts<500){	// 500 Durchläufe zur Bestimmung der Hälfte
   			// der Amplitude
   			
      /* Neuer Algorithmus: "Durchlaufende Werte" */
      OdometrieData(odoData);
      
      odoMemL[0]=odoMemL[1];
      odoMemL[1]=odoMemL[2];
      odoMemL[2]=odoData[0];
      
      odoMemR[0]=odoMemR[1];
      odoMemR[1]=odoMemR[2];
      odoMemR[2]=odoData[1];

      if(odoData[0]>lrHi[0]){       // die Höchstwerte ermitteln LINKS
         lrHi[0]=odoData[0];}
      else if(odoData[0]<lrLo[0]){  // die Tiefstwerte ermitteln
         lrLo[0]=odoData[0];}

      if(odoData[1]>lrHi[1]){       // die Höchstwerte ermitteln RECHTS
         lrHi[1]=odoData[1];}
      else if(odoData[1]<lrLo[1]){  // die Tiefstwerte ermitteln
         lrLo[1]=odoData[1];}
           
      testErg[0]=(lrHi[0]+lrLo[0])/2; // Test Ergebnisse zuweisen,
      testErg[1]=(lrHi[1]+lrLo[1])/2; // Mitte der Funktion bei Hälfte der Amplitue

      /* Hier wird geschaut, ob die Schwelle übertreten wird, falls ja hat Umdrehung stattgefunden */
      /* links */
      if((odoMemL[0]>odoMemL[1] && odoMemL[1]>odoMemL[2]) && (((odoMemL[1]/MITTEL)<1.05) && ((odoMemL[1]/MITTEL)>0.95)))
      {
         cntsL++;
      }
      else if(((odoMemL[0]<odoMemL[1]) && odoMemL[1]<odoMemL[2]) && (((odoMemL[1]/MITTEL)<1.05) && ((odoMemL[1]/MITTEL)>0.95)))
      {
         cntsL++;
      }

      /* rechts */
      if((odoMemR[0]>odoMemR[1] && odoMemR[1]>odoMemR[2]) && (((odoMemR[1]/MITTER)<1.05) && ((odoMemR[1]/MITTER)>0.95)))
      {
         cntsR++;
      }
      else if((odoMemR[0]<odoMemR[1] && odoMemR[1]<odoMemR[2]) && (((odoMemR[1]/MITTER)<1.05) && ((odoMemR[1]/MITTER)>0.95)))
      {
         cntsR++;
      }

      if(cntsL>420 && cntsR>420){
      alr=1;
      StatusLED(RED);
      schlafen(250);
      StatusLED(GREEN);}
      cnts++;
   }//while cnts

   MotorDir(BREAK,BREAK);
   MotorSpeed(0,0);

   schlafen(250);
   StatusLED(GREEN);
   schlafen(250);
   StatusLED(YELLOW);
   schlafen(250);
   StatusLED(GREEN);
   schlafen(250);
   StatusLED(YELLOW);
   schlafen(250);
   StatusLED(GREEN);
   schlafen(250);
   StatusLED(YELLOW);
   schlafen(250);
   StatusLED(GREEN);
   schlafen(250);

return alr;
}


void Forward(void){

if(!alr2){		// Beschleunigen damit er in Fahrt Werte sammelt
   MotorDir(FWD,FWD);   
   MotorSpeed(spd0,spd1);
   schlafen(250);	// Zeit zum Beschleunigen
   alr2 = 1;
}


if(alr)
{    // Falls Vorprüfung stattgefunden hat, ausführen
      
   /* Dieser Programmteil ist weitestgehend identisch mit InitFwd(); */
   /* Neuer Algorithmus: "Durchlaufende Werte" */      
   OdometrieData(odoData);
      
   odoMemL[0]=odoMemL[1];
   odoMemL[1]=odoMemL[2];
   odoMemL[2]=odoData[0];
      
   odoMemR[0]=odoMemR[1];
   odoMemR[1]=odoMemR[2];
   odoMemR[2]=odoData[1];

   if(odoData[0]>lrHi[0]){		// die Höchstwerte ermitteln LINKS
      lrHi[0]=odoData[0];}
   else if(odoData[0]<lrLo[0]){		// die Tiefstwerte ermitteln
      lrLo[0]=odoData[0];}

   if(odoData[1]>lrHi[1]){		// die Höchstwerte ermitteln RECHTS
      lrHi[1]=odoData[1];}
   else if(odoData[1]<lrLo[1]){		// die Tiefstwerte ermitteln
      lrLo[1]=odoData[1];}
      
   testErg[0]=((lrHi[0]+lrLo[0])/2);	// Testergebnisse zuweisen,
   testErg[1]=((lrHi[1]+lrLo[1])/2);	// Mitte der Odometriewertekurve

      /* Hier wird geschaut, ob die Schwelle übertreten wird, falls ja hat Umdrehung stattgefunden */
      /* links */
   if((odoMemL[0]>odoMemL[1] && odoMemL[1]>odoMemL[2]) && (((odoMemL[1]/testErg[0])<1.05) && ((odoMemL[1]/testErg[0])>0.95)))
   {
      cntsL++;
   }
   else if((odoMemL[0]<odoMemL[1] && odoMemL[1]<odoMemL[2]) && (((odoMemL[1]/testErg[0])<1.05) && ((odoMemL[1]/testErg[0])>0.95)))
   {
      cntsL++;
   }

   /* rechts */
   if((odoMemR[0]>odoMemR[1] && odoMemR[1]>odoMemR[2]) && (((odoMemR[1]/testErg[1])<1.05) && ((odoMemR[1]/testErg[1])>0.95)))
   {
      cntsR++;
   }
   else if((odoMemR[0]<odoMemR[1] && odoMemR[1]<odoMemR[2]) && (((odoMemR[1]/testErg[1])<1.05) && ((odoMemR[1]/testErg[1])>0.95)))
   {
      cntsR++;
   }

   /* Dies ist ein neuer Programmteil. */
   if(spd1==255 || spd0==255)	// Falls Maximalspd erreicht Spielraum schaffen
   { 
      spd1=spd1-10;
      spd0=spd0-10;
   }

   /* Hier wird die Geschwindigkeit angepasst. */
   if (cntsL>cntsR)     spd1++;
   else if(cntsL<cntsR) spd0++;

   /* Variable darf nicht überlaufen */
   if((cntsR>5000 || cntsL>5000) && (cntsR>3000 && cntsL>3000))
   {
      cntsL=cntsL-2000;
      cntsR=cntsR-2000;
   }

   /* Und losfahren! */
   MotorDir(FWD,FWD);
   MotorSpeed(spd0,spd1);

}
}

/* ####################################################### */
/* ############ BEGINN DES HAUPTPROGRAMMS ################ */
/* ####################################################### */

int main (void){

Init();		// Prozessor initialisieren
InitFwd();	// Odo Initialisieren

while(1)
{
   Forward();
}

return 0;
}

/* ####################################################### */
/* ############### ENDE DES PROGRAMMS #################### */
/* ####################################################### */