main_datei_in.cpp

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

#include "main_Datei_in.h"

ofstream Outfile;
ofstream Testfile;

//****************************************************************
int main(void)
{
	int Daten_Ende = 0;		//Ende der emfangenen Daten (aus Datei)

	//Initialisierung Datenarray (n_max Zeilen, 8 Spalten)
	for(n=0; n<n_max; n++)
	{
		for(int s=0; s<8; s++)
		{
			Daten[n][s]= 0;
		}
	}
	//Daten einlesen
	Datei_lesen();
	Daten_Ende = n-1;		//Ende der Datenzeile (letze ignorieren)
	n = 1;					//n auf erste Datenzeile setzen

	Simulationszeit = Daten[Daten_Ende][s_Zeit];
	cout<<"Simulationszeit[s]: " << Simulationszeit << endl;

	Outfile.open("Zeppelin.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" 
			<< "virtuell links" << "\t" << "virtuell_rechts" << "\t"
			<< "Weg" << "\t" << "v_Modell"<< "\t"
			<< "v_Fusion" << "\t" << "Abschnitt" << "\t" << "Korre" "\t"
			<< "Steigung" << endl;

virtuell_init();

while (Zeit < Simulationszeit)
{
int i = 0;


	float Mittelwert = 0;
	if (Task_fertig == 0)
	{
		switch (Taskaufruf)
		{
		case 1: delta_Richtung = 0;						//delta_Richtung aus alten Zyklus l鰏chen

				DatenShift(Messung_h, 5);
				Messung_h[0] = Daten[n][s_hinten];
				//Mittelwert = MittelwertFilter(Messung_h);
				Mittelwert = Messung_h[0];
				Kalman_Filter(Entf_hinten, Mittelwert, Gain_hinten, P_min_hinten);//Messwert Filtern
				
				Task_fertig	= 1; break;
	
		case 2:			
				DatenShift(Messung_l, 5);
				Messung_l[0] = Daten[n][s_links];
				Mittelwert = MittelwertFilter(Messung_l);
				//Mittelwert = Messung_l[0];
				Kalman_Filter(Entf_links, Mittelwert, Gain_links, P_min_links);//Messwert Filtern
				Task_fertig	= 1; break;
	
		case 3:	
				DatenShift(Messung_r, 5);
				Messung_r[0] = Daten[n][s_rechts];
				//Mittelwert = MittelwertFilter(Messung_r);
				Mittelwert = Messung_r[0];
				Kalman_Filter(Entf_rechts, Mittelwert, Gain_rechts, P_min_rechts);//Messwert Filtern
				Task_fertig	= 1; break;
	
		case 4:	DatenShift(Messung_o, 5);
				Messung_o[0] = Daten[n][s_oben];
				//Mittelwert = MittelwertFilter(Messung_o);
				Mittelwert = Messung_o[0];
				Kalman_Filter(Entf_oben, Mittelwert, Gain_oben, P_min_oben);//Messwert Filtern

				Task_fertig	= 1; break;
	
		case 5:	DatenShift(Messung_u, 5);
				Messung_u[0] = Daten[n][s_unten];		
				//Mittelwert = MittelwertFilter(Messung_u);
				Mittelwert = Messung_u[0];
				Kalman_Filter(Entf_unten, Mittelwert, Gain_unten, P_min_unten);//Messwert Filtern
				Task_fertig	= 1; break;
	
		case 6:	DatenShift(Messung_v, 5);
				Messung_v[0] = Daten[n][s_vorne];
				//Mittelwert = MittelwertFilter(Messung_v);
				Mittelwert = Messung_v[0];
				Kalman_Filter(Entf_vorne, Mittelwert, Gain_vorne, P_min_vorne);//Messwert Filtern
					
				Richtung_ist[0] += delta_Richtung;
				Position_bestimmen();
				Hoehe_ist = Entf_unten[0];				//Messwert unten wird Hoehe_ist
				Zeit += Prozesszyklus;	//globale Zeit um Prozesszyklus erh鰄en
				//	Kameradaten einlesen	
		
				//um ein 躡erlaufen der Richtungsdaten zu verhindern
				if (Richtung_ist[0] > 180)
				{
					Richtungs_Orientierung -= 360;
					Richtung_soll -= 360; 
					Richtung_ist[0] -= 360;
				}				
				if (Richtung_ist[0] < -180)
				{
					Richtungs_Orientierung += 360; 
					Richtung_soll += 360; 
					Richtung_ist[0] += 360;
				}				
				if (Sonderfall()){ FlugRegelung(); }
				else
				{
					if (Regel_Sperrzeit > 0){ Regel_Sperrzeit--; Richtung_ist[0] =0; }
					else
					{
//						if (Zeit > 90)
//							StartpunktAnfliegen();					
						virtuelle_Sensoren();
						Strategie();
						FlugRegelung();
					}
				}
				//----------------------------------------------------------------------------------
				//Ausgabe in Excel Datei
				Outfile << Zeit << "\t" << x << "\t" << x_Position[0] << "\t" 
				<< x <<"\t" << y_Position[0] << "\t"
				<< Hoehe_soll << "\t" << x << "\t" << Hoehe_ist << "\t"
				<< Richtung_soll << "\t" << x << "\t" << Richtung_ist[0] << "\t"
				<< HauptMotor_Leistung << "\t" << Servo_Winkel << "\t" << HeckMotor_Leistung << "\t"<< Handlung << "\t" 
				<< Messung_v[0] << "\t"<< Entf_vorne[0] << "\t"
				<< Messung_h[0] << "\t"<< Entf_hinten[0]<< "\t" 
				<< Messung_o[0] << "\t"<< Entf_oben[0]<< "\t" 
				<< Messung_u[0] << "\t"<< Entf_unten[0]<< "\t" 
				<< Messung_r[0] << "\t"<< Entf_rechts[0]<< "\t" 
				<< Messung_l[0] << "\t"<< Entf_links[0] << "\t" << virtuell_links << "\t" 
				<< virtuell_rechts << "\t" 
				<< s_Zeppelin << "\t" << v_Modell << "\t"
				<< v_Zeppelin[0] << "\t" << x << "\t" << fabs(Korrelations_Koeffizient(MessDaten_links)) <<"\t"
				<< Steigungs_Koeffizient(MessDaten_links) << endl;
				//----------------------------------------------------------------------------------

			n++;
			Task_fertig = 1;break;

	default: Task_fertig = 1;break;
	}//case Ende
	}//if Task_fertig

	//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	
	//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;

}

//**********************************************************************
void Datei_lesen()
{
	string Text;
	
	n=0;
	ifstream fin;//("Messwerte.txt");
	fin.open("Messwerte.txt");
	cout << "Hier der Inhalt der Datei:\n\n";
	cout.precision(3);
	
	while (fin.good() && n<n_max) 
	{
		int Spalte = 0;		//16 Spalten zum Auslesen (0..7)
							//Zeit, vorne, hinten, oben, unten, rechts, links, Gyro

		while (Spalte < 8)
		{			
			if (n==0)
			{
				cout.width(6);
				fin >> Text;
				cout << Text << " | ";

			}
			else
			{
				if (Spalte < 8)
				{
					cout.width(6);
					fin >> Daten[n][Spalte];
					cout << Daten[n][Spalte] << " | ";
				}
			}
			Spalte++;
		}
		cout << endl;
		n++;
	}
	n -= 1;					//wenn Dateiende, dann n++ wieder r點kg鋘g
	fin.close();

}