verhalten.cpp

this the code of bootstation

#if _MSC_VER > 1000
#include "stdafx.h"		//in MFC einkommentieren, sonst raus!


#include "Verhalten.h"



//Verhalten des Luftschiffes pro Prozesszyklus
void Verhalten(double Servo, double HauptMotor, double HeckMotor)
{

	double Kraft_y;
	double Kraft_y_wirk;
	double Kraft_x;
	double Kraft_x_wirk;
	double Kraft_z;
	double Kraft_z_wirk;

	double a_y;
	double a_z;
	double a_x;
	double v_x;
	double v_y;
	double v_z;
	double Richtung_rad;

	//Begrenzung auf physikalische maximal Werte
	if (Servo > 180) Servo = 180;
	if (Servo < 0) Servo = 0;
	if (HauptMotor > 0.7) HauptMotor = 0.7;
	if (HauptMotor < 0) HauptMotor = 0;
	if (HeckMotor > 0.25) HeckMotor = 0.25;
	if (HeckMotor < -0.25) HeckMotor = -0.25;
	if (fabs(HeckMotor) < 0.08) HeckMotor = 0;

	//Begrenzung auf realistische Startwerte
	if (HauptMotor < 0.1) HauptMotor = 0;
	if (fabs(HeckMotor) < 0.1) HeckMotor = 0;
	
	Richtung_rad = Richtung_grd *Umfang/360;


	for(int i=0; i<SchrittProTakt/AufrufeProZyklus; i++)
	{		
		//Tr鋑heit der Servo- und Motorsysteme
		Servo_real += (Servo - Servo_real)*(1 - exp(-Prozesszyklus/SchrittProTakt/Tau));
		HauptMotor_real += (HauptMotor - HauptMotor_real)*(1 - exp(-Prozesszyklus/SchrittProTakt/Tau));
		HeckMotor_real += (HeckMotor - HeckMotor_real)*(1 - exp(-Prozesszyklus/SchrittProTakt/Tau));

		Kraft_y = sin(Servo_real*Pi/180)*HauptMotor_real*FmaxMotor*2;
		Kraft_x = cos(Servo_real*Pi/180)*HauptMotor_real*FmaxMotor*2;
		Kraft_z = HeckMotor_real * FmaxMotor;
		
		//Verhalten in x Bewegungsrichtung
		Kraft_x_wirk = Kraft_x - (v_x_alt * Luft_x);	//Wirksame Kraft in x Richtung
		a_x = Kraft_x_wirk/Gewicht;						//Beschleunigung in x Richtung
		v_x = v_x_alt + (a_x * Prozesszyklus/SchrittProTakt); //Geschwindigkeit in x Richtung
		Weg_m = Weg_alt + (v_x * Prozesszyklus/SchrittProTakt); //Zur點kgelegter Weg in x Richtung
		v_x_alt = v_x;

		//Verhalten in y Bewegungsrichtung
		Kraft_y_wirk = Kraft_y - (v_y_alt * Luft_y) - (Gravitation*Abtrieb*Gewicht);//Wirksame Kraft in y Richtung
		a_y = Kraft_y_wirk/Gewicht;						//Beschleunigung in y Richtung
		v_y = v_y_alt + (a_y * Prozesszyklus/SchrittProTakt); //Geschwindigkeit in y Richtung
		Hoehe_m = Hoehe_alt + (v_y * Prozesszyklus/SchrittProTakt); //Zur點kgelegter Weg in y Richtung
		v_y_alt = v_y;
		if (Hoehe_m < 0) Hoehe_m = 0;					//kleiner 0 nicht m鰃lich
		Hoehe_alt = Hoehe_m;

		//Verhalten bei Drehung
		Kraft_z_wirk = Kraft_z - (v_z_alt * Luft_z);//Wirksame Kraft in z Richtung
		a_z = Kraft_z_wirk *2/Dreh_Traegheit;//Beschleunigung in z Richtung
		v_z = v_z_alt + (a_z * Prozesszyklus/SchrittProTakt); //Geschwindigkeit in z Richtung
		Richtung_rad = (Richtung_alt + (v_z * Prozesszyklus/SchrittProTakt)); //Zur點kgelegter Weg in z Richtung
		v_z_alt = v_z;
		Richtung_alt = Richtung_rad;
	
		Richtung_grd = Richtung_rad *360/Umfang;

	Pos_y += (Weg_m - Weg_alt)* cos(Richtung_grd/180*Pi);
	Pos_x += (Weg_alt - Weg_m)* sin(Richtung_grd/180*Pi);

	Weg_alt = Weg_m;
	}




}