position.cpp

来自「this the code of bootstation」· C++ 代码 · 共 327 行

#include "stdafx.h"		//in MFC einkommentieren, sonst raus!


#include "Position.h"


//******************************************************************************************
void Position_bestimmen()
{
	float temp_Weg;

	Geschwindigkeit_bestimmen();
//	Winkel_korrigieren();
	//zur點kgelegter Weg in diesem Prozesszyklus
	temp_Weg = 0.5*(v_Zeppelin[0] + v_Zeppelin[1])*Prozesszyklus;
	s_Zeppelin += temp_Weg;

	//alte Koordinaten verschieben
		x_Position[4] = x_Position[3];
		x_Position[3] = x_Position[2];
		x_Position[2] = x_Position[1];
		x_Position[1] = x_Position[0];
		y_Position[4] = y_Position[3];
		y_Position[3] = y_Position[2];
		y_Position[2] = y_Position[1];
		y_Position[1] = y_Position[0];
	
	//Weg in diesem Zyklus in X und Y Antei aufspalten und zur alten Position rechnen
	x_Position[0] = x_Position[1] + temp_Weg * -sin(Richtung_ist[0]*Pi/180);
	y_Position[0] = y_Position[1] + temp_Weg * cos(Richtung_ist[0]*Pi/180);

}
int x;
//******************************************************************************************
//Position im Raum (x, y Koordinate durch Geschwindigkeitsmessung und Richtung bestimmen)
void Geschwindigkeit_bestimmen()
{
	bit G = false;				//G黮tigkeit des Messwertes	
	float v_Mess_v;	//gemessene Geschwindigkeit vorne (m/s)
	float v_Mess_h;	//gemessene Geschwindigkeit hinten (m/s)
	float v_Mess;		//aufgrund Plausibilit鋞skontrolle angenommene tats鋍hliche Geschw.

	v_Mess_v = (Messung_v[1] - Messung_v[0])/Prozesszyklus;//gemessene Geschwindigkeit vorne (m/s)
	v_Mess_h = (Messung_h[0] - Messung_h[1])/Prozesszyklus;//gemessene Geschwindigkeit hinten (m/s)

	 //Modellberechnung: delta_v (Geschwindikeits鋘derung wird ermittelt)
	Modell(ServoWinkel, HauptMotorLeistung);

//--> Grunds鋞zlich plausibel <--
	//min. ein Sensor hat Messkontakt 
	//&& min. ein Sensor ist Plausibel
	//&& (Handlung ist Fwd || Rev || Koor_Fwd_Rev)
	//plausibel -> ist kleiner als 1.5m/s Abweichung vom Modell
	if ( (Messung_v[0] < max_Sensor_v || Messung_h[0]< max_Sensor_h) 
		&& 	(fabs(v_Mess_v - v_Modell) < 1.5
		    || fabs(v_Mess_h - v_Modell) < 1.5)
		&& (Handlung == Fwd || Handlung == Rev || Handlung == Koor_Fwd_Rev) )
	{
	//--> Beide Daten verwendbar <--
		//Beide innerhalb der Reichweite
		if (Messung_v[0] < max_Sensor_v && Messung_h[0] < max_Sensor_h )
		{
		//--> Beide Messwerte plausibel <--
			//wenn Geschwindikeitsdifferenz < 0.3m/s (vorne-hinten)
			if (fabs(v_Mess_v - v_Mess_h) < 0.3)
			{
				v_Mess = (v_Mess_v + v_Mess_h)*0.5;	//Mittelwert aus beiden Geschwindigkeiten
				G = true;//Messwert ist g黮tig
			}
		//--> ein Messwert plausibel <--
			else
			{
				//wahrscheinlicheren Messwert nehmen
				if (  fabs(v_Mess_v - v_Modell) 
					< fabs(v_Mess_h - v_Modell) )
				{
					v_Mess = v_Mess_v;	//vordere Geschwindigkeit wahrscheinlich
					G = true;			//Messwert ist g黮tig
				}
				else
				{
					v_Mess = v_Mess_h;	//hintere Geschwindigkeit wahrscheinlich
					G = true;			//Messwert ist g黮tig
				}			
			}
		}

		else
		{
		//--> ein Datum verwendbar && plausibel<--
			if (Messung_v[0] < max_Sensor_v && fabs(v_Mess_v - v_Modell) < 1.5) 
			{
				v_Mess = v_Mess_v;	//vordere Geschwindigkeit g黮tig
				G = true;			//Messwert ist g黮tig
			}
			//--> hintere Messwert <--
			else
			{
				if (Messung_h[0] < max_Sensor_h && fabs(v_Mess_h - v_Modell) < 1.5)
				{
					v_Mess = v_Mess_h;	//hintere Geschwindigkeit g黮tig
					G = true;			//Messwert ist g黮tig
				}
			//--> kein Datum verwendbar <--
				else
				{
					G = false;			//Messwert ist g黮tig
				}
			}	
		}
	}
//--> Grunds鋞ztlich nicht plausibel <--
	else
	{
		G = false;					//Messwert ist ung黮tig	
	}
	//Datenfusion vollziehen
	Kalman_Datenfusion(v_Zeppelin, delta_v, v_Mess, G, P_v, Q_v);
}//Funktion


//*******************************************************************************************
//bestimmt Anhand des Servowinkels und der 
//Hauptmotorleistung eine Modell-Geschwindikeits鋘derung
void Modell (float Servo, float HauptMotor)
{
	float Kraft_x;
	float Kraft_x_wirk;
	float a_Modell;

	//Begrenzung auf physikalische maximal Werte
	if (Servo > 180) Servo = 180;
	if (Servo < 0) Servo = 0;
	if (HauptMotor > 1) HauptMotor = 1;
	if (HauptMotor < 0) HauptMotor = 0;

	//Begrenzung auf realistische Startwerte
	if (HauptMotor < 0.1) HauptMotor = 0;

	
	//Tr鋑heit der Servo- und Motorsysteme
	Servo_real_M += (Servo - Servo_real_M)*(1 - exp(-Prozesszyklus/Tau));
	HauptMotor_real_M += (HauptMotor - HauptMotor_real_M)*(1 - exp(-Prozesszyklus/Tau));
	
	Kraft_x = cos(Servo_real_M*Pi/180)*HauptMotor_real_M*FmaxMotor*2;
		
	//Verhalten in x Bewegungsrichtung
	Kraft_x_wirk = Kraft_x - (v_Modell * Luft_x);			//Wirksame Kraft in x Richtung
	a_Modell = Kraft_x_wirk/Gewicht;						//Beschleunigung in x Richtung
	delta_v = 0.5*(a_Modell_alt + a_Modell)*Prozesszyklus;	//delta_v in m/s
	v_Modell += delta_v;



}//Funktion

//*******************************************************************************************
//Den berechneten Winkel vom Gyroskop korrigieren. (Richtung_ist) 
//Geschwindigkeit muss bekannt sein --> vorher Position bestimmen
void Winkel_korrigieren()
{
	float v_links;			//Geschwindigkeit zur linken Wand (m/s)
	float v_rechts;		//Geschwindigkeit zur rechten Wand (m/s)
//	float Winkel_zur_Wand; //berechtete Ausrichtung zur Wand
	float Winkel_zur_Wand_l;//berechtete Ausrichtung zur Wand 黚er linken Sensor
	float Winkel_zur_Wand_r;//berechtete Ausrichtung zur Wand 黚er rechten Sensor	
	int i;
	float v_rechts_korr;
	float v_links_korr;

	for(i=9; i>0; i--)
	{
		MessDaten_links[i] = MessDaten_links[i-1];
	}
	MessDaten_links[0] = Messung_l[0];
	
	for(i=9; i>0; i--)
	{
		MessDaten_rechts[i] = MessDaten_rechts[i-1];
	}
	MessDaten_rechts[0] = Messung_r[0];

	//Geschwindigkeit zur linken Wand (m/s)
	v_links_korr = Korrelations_Koeffizient(MessDaten_links);
	v_rechts_korr = Korrelations_Koeffizient(MessDaten_rechts);

	//Plausibilit鋞spr黤ung
	//nur berechnen, wenn ausreichend Vorw鋜tsgeschw. (> 0.2m/s)
	//&& Handlung Fwd || Rev || Koor_Fwd_rev ist 
	if ( fabs(v_Zeppelin[0]) > 0.2 
		 && (Handlung == Fwd || Handlung == Rev || Handlung == Koor_Fwd_Rev))
	{
		
		if (v_links_korr > 0.91 && Messung_l[0] < max_Sensor_l && v_links_korr <= 1)
		{
			v_links = -Steigungs_Koeffizient(MessDaten_links);
			Winkel_zur_Wand_l = (atan(v_links/v_Zeppelin[0]) *180/Pi);
			Winkel_zur_Wand_l += Richtungs_Orientierung;
			Richtung_ist[0] = Winkel_zur_Wand_l;
		}
		else
		{
			if (v_rechts_korr > 0.91 && Messung_r[0] < max_Sensor_r && v_rechts_korr <= 1)
			{
				v_rechts = Steigungs_Koeffizient(MessDaten_rechts);
				Winkel_zur_Wand_r = (atan(v_rechts/v_Zeppelin[0]) *180/Pi);
				Winkel_zur_Wand_r += Richtungs_Orientierung;
				Richtung_ist[0] = Winkel_zur_Wand_r;
			}		
		}
	}
}

//Funktion lineare Regression, Steigungskoeffizient bestimmen
float Steigungs_Koeffizient(float Daten[10])
{
	float Zaehler;
	float Nenner;		//Prozesszyklus ist Konastant (0.2), daher auch Konstante
	int	i;
	float XiYi	= 0;
	float Yi	= 0;
	float Xi	= 0;
	float XiXi	= 0;

	for(i=0; i<10; i++)
	{
		XiYi += Daten[i] * Prozesszyklus * i;
	}
	for(i=0; i<10; i++)
	{
		Yi += Daten[i];
	}

	for(i=0; i<10; i++)
	{
		Xi += Prozesszyklus * i;
	}
	
	Zaehler = (10 * XiYi) - (Yi * Xi);

	for(i=0; i<10; i++)
	{
		XiXi += Prozesszyklus*i * Prozesszyklus*i;
	}

	Nenner = (10 * XiXi) - (Xi * Xi);

	return (-Zaehler/(Nenner));
}

float Korrelations_Koeffizient(float Daten[10])
{
	float Zaehler;
	float Nenner;		//Prozesszyklus ist Konastant (0.2), daher ua Konstante eingesetzt
	int	i;
	float XiYi	= 0;
	float Yi	= 0;
	float Xi	= 0;
	float YiYi	= 0;
	float XiXi	= 0;

	for(i=0; i<10; i++)
	{
		XiYi += Daten[i] * Prozesszyklus * i;
	}
	for(i=0; i<10; i++)
	{
		Yi += Daten[i];
	}
	for(i=0; i<10; i++)
	{
		Xi += Prozesszyklus * i;
	}

	Zaehler = (10 * XiYi) - (Yi * Xi);

	for(i=0; i<10; i++)
	{
		YiYi += Daten[i] * Daten[i];
	}
	for(i=0; i<10; i++)
	{
		XiXi += Prozesszyklus*Prozesszyklus * i*i;
	}

	Nenner = ((10 * XiXi)-(Xi*Xi)) * ((10 * YiYi) - (Yi * Yi));
	Nenner = sqrt(Nenner);

	return(Zaehler/Nenner);
}

float Abschnitts_Koeffizient(float Daten[10])
{
	float Zaehler;
	float Nenner;		//Prozesszyklus ist Konastant (0.2), daher ua Konstante eingesetzt
	int	i;
	float XiYi	= 0;
	float Yi	= 0;
	float Xi	= 0;
	float YiYi	= 0;
	float XiXi	= 0;	
	
	for(i=0; i<10; i++)
	{
		XiXi += Prozesszyklus*Prozesszyklus * i*i;
	}
	for(i=0; i<10; i++)
	{
		Yi += Daten[i];
	}
	for(i=0; i<10; i++)
	{
		Xi += Prozesszyklus * i;
	}
	for(i=0; i<10; i++)
	{
		XiYi += Daten[i] * Prozesszyklus * i;
	}

	Zaehler = (XiXi * Yi) - (Xi * XiYi);

	Nenner = (10 * XiXi) - (Xi * Xi);

	return(Zaehler/Nenner);
}