main_steuerungsoftware.cpp

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

#include "stdafx.h"
#include "main_Steuerungssoftware.h"


//ohne Testfeld! Sensordaten werden aus Daten.xls gelesen!

void main_Controller(float Sensordaten[], unsigned char *CamDaten);
//****************************************************************
void main_Controller(float *Sensordaten, unsigned char *CamDatenFromCom)
{

for (int i=0; i<8; i++){CamDaten[i] = CamDatenFromCom[i];}
float Mittelwert;
	
	Taskaufruf = 1;
	for (int Aufruf = 0; Aufruf < 6; Aufruf++)
	{

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

			Task_fertig	= true; break;
			

	default: break;
	}//case Ende


	//manueller Aufruf der ISR (ist sp鋞er Timer-Interrupt)
	ISR();	
	//-------------------------------------------------------------------------------------

	}//for
}//Funktion

//********************************************************************
bool Sonderfall()
{
	if (Entf_hinten[0] < 0.4)
	{
		Leistung_x_soll = 0.6;
		return true;
	}

return (false);
}

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

//bei neuem Flug alle Variablen auf ihre Initialisierung setzen 
void Init_Variablen()
{
//FlugroutenGenerator
P_Cam_Richtung = 0.4;		//P Faktor f黵 Umwandlung Pixel-Mitte-Diff. in Sollwinkel
Flug_gestoppt = false;
Winkel_diff	= 0;			//Winkeldifferenz zur eigenen Ausrichtung
Stoppunkt;

P_Kamera_Leistung_x = 0.002;

//Kalman
P_v[0]		= 1;	//Fehler-Kovarianz bei Kalman_Datenfusion Geschwindigkeit
P_Winkel[0]	= 1;	//Fehler-Kovarianz Kalman_Datenfusion Winkelkorrektur
//-----------------------------------------------------------------------------------------
//temp Variablen
P_min_vorne[0]	= 1;
P_min_hinten[0] 	= 1;
P_min_oben[0]		= 1;
P_min_unten[0]	= 1;
P_min_rechts[0] 	= 1;
P_min_links[0]	= 1;

//Koordinaten
neue_Koordinaten	= false;	//neue Koordinaten berechen (auf true setzen)
x_Koordinate		= 0;		//Zielkoordinate X
y_Koordinate		= 0;		//Zielkoordinate Y
//Ausgabe
Koord_erreichbar	= false;	//Zielkoordinate erreichbar? (false, wenn nicht)
Punkt_erreicht		= true;		//true, wenn Punkt erreicht ist oder in dessen Nahbereich (<2m)
Stop_Handlung		= false;	//eine Stop_Handlung wurde ausgef黨rt (dann true)
StopZeit			= 0;
//-----------------------------------------------------------------------------------------
float e_Entf_Stop_alt	= 0;		//alte Entfernung

//Main
ServoWinkel				= 0;	//auf diesen Wert wird der Servo angesteuert
HauptMotorLeistung		= 0;	//mit diesem Wert wird der Moror angesteuert
Simulationszeit		= 100;		//Simulationszeit in Sekunden

//allgemeine globale Variablen
//-----------------------------------------------------------------------------------------
Taskaufruf		= 1;		//Nummer des Auszuf黨renden Task (0==kein Task)
Task_fertig		= false;
Zeit			= 0;		//globale Zeit seit Start (erh鰄t sich um den Prozesszyklus)
delta_Richtung	= 0;
Zeit_Datei		= 0;		//eingelesene Zeit aus Datei

//Position
//temp Variablen
Messung_v_alt	= 0;	//Entfernungsmessung vorne vom letzten Zyklus
Messung_h_alt	= 0;	//Entfernungsmessung hinten vom letzten Zyklus
Messung_l_alt	= 0;	//Entfernungsmessung links vom letzten Zyklus
Messung_r_alt	= 0;	//Entfernungsmessung rechts vom letzten Zyklus
Servo_real_M		= 0;//Tempvariable zum Berechnen der Servostellung nach e-Funktion
HauptMotor_real_M	= 0;//Tempvariable zum Berechnen der Motorleistung nach e-Funktion
a_Modell_alt	= 0;	//berechnete Modellbeschleunigung (aus letztem Zyklus)

//Regelung
Hoehe_soll			= 0;					
Hoehe_ist				= 0;
Leistung_x_soll		= 0;	//bestimmter Wert f黵 die Motorleistung	
Richtung_soll			= 0;	//Soll_Richtung des Zeppelins
memset(Richtung_ist, 0, 5);	//Richtung des Zeppelins (bezogen auf Startausrichtung 0