main_modellsim.cpp

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

#include "main_ModellSim.h"

ofstream Outfile;

//****************************************************************
int main(void)
{

	cout<<"Simulationszeit[s]: ";
	cin>>Simulationszeit;

	Outfile.open("Zeppelin_Modell.xls");
	Outfile << "Zeit[s]" << "\t" << "Pos_x (Modell)" << "\t" << "x_Pos (Kalman)" << "\t"
			<< "Pos_y (Modell)" <<"\t" << "y_Pos (Kalman)" << "\t"
			<< "H鰄e_soll" << "\t" << "H鰄e_Modell" << "\t" << "H鰄e_ist" << "\t" 
			<< "Richtung_soll" << "\t" << "Richtung_Modell" << "\t" << "Richtung_ist" << "\t"
			<< "HauptMotor" << "\t" << "Servo_Winkel" << "\t" << "HeckMotor" << "\t" << "Handlung" << "\t"
			<< "Sensor_vorne" << "\t" << "Entf_vorne" << "\t"
			<< "Sensor_hinten" << "\t" << "Entf_hinten" << "\t"	
			<< "Sensor_oben" << "\t" << "Entf_oben"<< "\t"
			<< "Sensor_unten" << "\t" << "Entf_unten"<< "\t"
			<< "Sensor_rechts" << "\t" << "Entf_rechts"<< "\t"
			<< "Sensor_links" << "\t" << "Entf_links" << "\t" << "v_Zeppelin" << endl;
	
while (Zeit < Simulationszeit)
{

	if (Zeit > 6)
	{
		Zeit=Zeit;
	}

	switch (Taskaufruf)
	{


	case 1: Messung_v = Sen_vorne_lesen();	//Messung starten, Messwert auslesen
			Kalman_Filter(Entf_vorne, Messung_v, Gain_vorne, P_min_vorne);//Messwert Filtern
			Task_fertig	= true; break;

	case 2:	Messung_h = Sen_hinten_lesen();	//Messung starten, Messwert auslesen	
			Kalman_Filter(Entf_hinten, Messung_h, Gain_hinten, P_min_hinten);//Messwert Filtern
			Task_fertig	= true; break;

	case 3:	Messung_o = Sen_oben_lesen();		//Messung starten, Messwert auslesen	
			Kalman_Filter(Entf_oben, Messung_o, Gain_oben, P_min_oben);//Messwert Filtern
			Task_fertig	= true; break;;

	case 4:	Messung_u = Sen_unten_lesen();	//Messung starten, Messwert auslesen		
			Kalman_Filter(Entf_unten, Messung_u, Gain_unten, P_min_unten);//Messwert Filtern
			Task_fertig	= true; break;

	case 5:	Messung_r = Sen_rechts_lesen();	//Messung starten, Messwert auslesen			
			Kalman_Filter(Entf_rechts, Messung_r, Gain_rechts, P_min_rechts);//Messwert Filtern
			Task_fertig	= true; break;

	case 6:	Messung_l = Sen_links_lesen();	//Messung starten, Messwert auslesen			
			Kalman_Filter(Entf_links, Messung_l, Gain_links, P_min_links);//Messwert Filtern
			
			Hoehe_ist = Entf_unten[0];
			
			//Zeit-Variable i um die Prozesszyklusdauer erh鰄en
			Zeit += Prozesszyklus;

			//膎derung der Richtung zum letzten Zyklus berechnen (Richtung_grd sp鋞er vom Gyro)
			Richtung_ist[0] = Richtung_grd;

			//delta_Richtung = Richtung_grd - Richtung_ist[0];
			//Geschwindigkeitsmessung, Winkelkorrektur und Positionsbestimmung
			Position_bestimmen();

			virtuelle_Sensoren();

		//----------------------------------------------------------------------------------
		//Ausgabe in Excel Datei
				Outfile << Zeit << "\t" << Pos_x << "\t" << x_Position[0] << "\t" 
				<< Pos_y <<"\t" << y_Position[0] << "\t"
				<< Hoehe_soll << "\t" << Hoehe_m << "\t" << Hoehe_ist << "\t"
				<< Richtung_soll << "\t" << Richtung_grd << "\t" << Richtung_ist[0] << "\t"
				<< HauptMotor_Leistung << "\t" << Servo_Winkel << "\t" << HeckMotor_Leistung << "\t"<< Handlung << "\t" 
				<< Messung_v << "\t"<< Entf_vorne[0] << "\t"
				<< Messung_h << "\t"<< Entf_hinten[0]<< "\t" 
				<< Messung_o << "\t"<< Entf_oben[0]<< "\t" 
				<< Messung_u << "\t"<< Entf_unten[0]<< "\t" 
				<< Messung_r << "\t"<< Entf_rechts[0]<< "\t" 
				<< Messung_l << "\t"<< Entf_links[0] << "\t" << v_Zeppelin[0] << "\t" 
				<< 	"x" << "\t" << delta_v << endl;
		//----------------------------------------------------------------------------------
			
			if (Sonderfall())
			{

			}
			else
			{
				if (Start_Sperrzeit > 0){ Start_Sperrzeit--; }
				else
				{
					if (Zeit > 80 ) StartpunktAnfliegen();
					Strategie();
					FlugRegelung();
				}
			}
			Task_fertig	= true;	break;

	default: break;
	}//case Ende


	//um ein theoretisch m鰃liches 躡erlaufen der Richtungsdaten zu verhindern
	if (Richtung_ist[0] > 180 || Richtung_grd > 180)
	{
		Richtungs_Orientierung -= 360; Richtung_soll -= 360; Richtung_ist[0] -= 360; Richtung_grd -= 360; 
		Richtung_alt = Richtung_grd/360*Umfang;
	}
	
	if (Richtung_ist[0] < -180 || Richtung_grd < -180)
	{
		Richtungs_Orientierung += 360; Richtung_soll += 360; Richtung_ist[0] += 360; Richtung_grd += 360; 
		Richtung_alt = Richtung_grd/360*Umfang;
	}




	//Testumgebung-------------------------------------------------------------------------
	//Schnittstelle zwischen der Regelung und dem physikalischem Verhalten
	//(wird 6x pro Zyklus aufgerufen --> AufrufeProZyklus=6 in Verhalten.h)
	Verhalten(Servo_Winkel, HauptMotor_Leistung, HeckMotor_Leistung);//physikalisches Verhalten
	//Testgel鋘de aufrufen	
	Testfeld();
	//manueller Aufruf der ISR (ist sp鋞er Timer-Interrupt)
	ISR();	
	//-------------------------------------------------------------------------------------




}//while Simulation

	Outfile.close();
return(0);
}//Funktion

//********************************************************************
bool Sonderfall()
{
return (false);
}

//*********************************************************************
void ISR()
{
	if (Taskaufruf > 5) Taskaufruf = 1;
	else Taskaufruf++;
	Task_fertig = false;

}