strategie.cpp

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

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



#include "Strategie.h"


void Strategie()
{
	switch (Handlung)
	{
	case Fwd:			Strategie_Vor();		break;
	case Turn:			Strategie_Dreh();		break;
	case Rev:			Strategie_Ruek();		break;
	case Koor_r_Turn:	Koor_rechts_Dreh();		break;
	case Koor_l_Turn:	Koor_links_Dreh();		break;
	case Koor_Fwd_Rev:	Koord_Vor_Ruek();		break;
	case Stop:			Flug_stoppen();			break;
	case ToOrigin:		ZumStartpunkt();		break;
	case FollowObject:	Objektverfolgung();		break;

	default:					 break;
	}	
}

//******************************************************************************
//Sollwertgenerierung Vorw鋞s: Leistung_x_soll, Hoehe_soll, Richtung_soll
void Strategie_Vor()
{
	
	float temp_P;	//Variablen f黵 Leistung_x_soll Berechnung (Geschwindigkeit)
	float temp_I;
	float temp_D;
	float temp_e;
	float temp_Erg;

	//Hoehe_soll ------------------------------------------------------------------
	Flug_Hoehe();

	//keine x_Bewegung, bei H鰄enunterschied zwischen Soll und Ist >1m (Strategie wird ausgesetzt)
	if (Hoehe_soll - Entf_unten[0] > 1)
	{
		Leistung_x_soll = 0;
	}
	//-----------------------------------------------------------------------------
	//sonst Strategie ausf黨ren
	else
	{

		if (Strategie_Sperrzeit > 0)
		{
			Strategie_Sperrzeit--; //Sperrzeit verringern, falls vorhanden
		}
		else
		{
			//Regelung der Geschwindigkeit mit Sollwert (1m) Abstand zum Hindernis vorne.

			temp_e 			= Entf_vorne[0] - Stop_vor_Wand;

		//Strategie鋘derung, wenn Sollwert praktisch erreicht und Geschwindigkeit praktisch 0
			if ((fabs(e_Entf_Fwd_alt - temp_e) < 0.02) && (fabs(temp_e) < 0.1))
			{
				//auf Drehm鰃lichkeit pr黤en und angemessene Entfernung
				if ( ((Entf_rechts[0] >= 2) && (Entf_links[0] >= 2)) &&
					 ((Entf_rechts[0] >= 3) || (Entf_links[0] >= 3)) )
				{
					Handlung = Turn;		
				}
				//sonst wieder R點kw鋜ts
				else
				{
					Handlung = Rev;
				}	
			}
			
			//sonst Sollwertberechnung f黵 Fwd Funktion
			else
			{
		//Leistung_x_soll ---------------------------------------------------------------
				temp_P 			= temp_e * P_Entf_Fwd;
				temp_I			= 0.5*(temp_e + e_Entf_Fwd_alt)*Prozesszyklus;
				temp_I 			= temp_I * I_Entf_Fwd + I_Anteil_Entf_Fwd;
				
				//Wenn e_Entfernung > 1m, keinen I_Anteil berechnen
				if (fabs(temp_e) > 1) temp_I = 0;				
				I_Anteil_Entf_Fwd= temp_I;

				temp_D			= (temp_e - e_Entf_Fwd_alt)/Prozesszyklus * D_Entf_Fwd;
				
				//Wenn e_Entfernung > 1m, keinen D_Anteil berechnen
				if (fabs(temp_e) > 1) temp_D = 0;
				
				e_Entf_Fwd_alt 	= temp_e;
				temp_Erg		= temp_P + temp_I + temp_D;
				
				if (temp_Erg >= 1){temp_Erg = 1;}
				if (temp_Erg <= -1){temp_Erg = -1;}
				
				//neuer Leistungssollwert
				Leistung_x_soll = temp_Erg;	
		//-------------------------------------------------------------------------------

		//Richtung_soll -----------------------------------------------------------------

				//Regeln des Seitenabstandes auf 2m, bei einer Gangbreite von >=4m, sonst mittig zum Gang
				
				//Regelung, wenn Seitenabstand > 4m
				if (Entf_rechts[0] + Entf_links[0] >=4)
				{
					//Regelminimum: Entfernung < 3m
					if ((Entf_rechts[0] < 3) || (Entf_links[0] < 3))
					{	
						//Regeln auf rechte Seite (2m)
						if (Entf_rechts[0] < Entf_links[0])
						{
							//Seitenregelung true==Linksdrehung, wenn soll-ist==pos. auf 2m
							PID_Seite(true, Entf_rechts, Entf_soll);
						}
						//Regeln auf linke Seite (2m)
						else
						{
							//Seitenregelung false==Rechtsdrehung, wenn soll-ist==pos. auf 2m
							PID_Seite(false, Entf_links, Entf_soll);
						}
					}
				}
				//Regelung, wenn Entfernung < 4m auf Sollabstand (R+L)/2
				else
				{
					//Seitenregelung true==Linksdrehung, wenn soll-ist==pos. auf (R+L)/2
					PID_Seite(true, Entf_rechts, (Entf_rechts[0]+Entf_links[0])/2);

				}
		//-----------------------------------------------------------------------------

			}//else Sollwertberechnug
		}//else Sperrzeit
	}//else Strategie ausf黨ren (H鰄e ist OK)
}//Funktion

//********************************************************************************
//Sollwertgenerierung Drehung: Leistung_x_soll=0, Hoehe_soll, Richtungs_Orientierung=+-90
void Strategie_Dreh()
{
	Leistung_x_soll = 0;

	if (Dreh_gestartet == true)
	{
		//wenn Richtung praktisch erreicht, dann n鋍hste Handlung ausf黨ren
		if(fabs(Richtung_ist[0] - Richtungs_Orientierung) < 3)
		{
			//n鋍hste Handlung ist Fwd
			temp_init();					//temp Variablen initialisieren
			Dreh_gestartet = false;
			Handlung = Fwd;
			Strategie_Sperrzeit = 1;		//Sperrzeit f黵 10 Zyklen (wg Istwert Erfassung)
		}
	}	
	//Drehung starten
	else
	{
		Dreh_gestartet = true;	//Drehung gestartet vermerken		
		Leistung_x_soll = 0;	//Vortrieb abschalten

		//Drehung in Richtung rechts
		if (Entf_rechts[0] > Entf_links[0])
		{
			Richtungs_Orientierung -= 90;
		}
		//Drehung in Richtung links
		else
		{
			Richtungs_Orientierung += 90;
		}
		Richtung_soll = Richtungs_Orientierung; //neue Orientierung dem Sollwert zuweisen
	}
}

//*******************************************************************************
//Sollwertgenerierung R點kwarts: Leistung_x_soll, Hoehe_soll, Richtung_soll
//bis Drehung m鰃lich oder Hindernis erreicht
void Strategie_Ruek()
{
	float temp_P;
	float temp_I;
	float temp_D;
	float temp_e;
	float temp_Erg;

	//Hoehe_soll ------------------------------------------------------------------
	Flug_Hoehe();

	//keine x_Bewegung, bei H鰄enunterschied zwischen Soll und Ist >1m (Strategie wird ausgesetzt)
	if (Hoehe_soll - Entf_unten[0] > 1)
	{
		Leistung_x_soll = 0;
	}
	//-----------------------------------------------------------------------------
	//sonst Strategie ausf黨ren
	else
	{
		temp_e 			= Entf_hinten[0] - temp_Entf_stop; //Stopenffernung (evtl. Seitenweg)

		//testen, ob eine Drehung sinnvoll ist (Seitenabstand > 4m)
		if( ((Entf_rechts[0] >= 4) || (Entf_links[0] >= 4)) && 
			((Entf_rechts[0] >= 2) && (Entf_links[0] >= 2))	&& 
			(Seitenweg_gesehen == false) && 
			(Entf_hinten[0] < 6) )
		{
			//Wenn noch kein Seitenweg gesehen wurde, diesen vermerken und nach 50cm stoppen
			temp_Entf_stop = Entf_hinten[0] - 0.5;	
			Seitenweg_gesehen = true;
		}

		//Strategie鋘derung, wenn Sollwert praktisch erreicht und Geschwindigkeit praktisch 0
		if ((fabs(e_Entf_Fwd_alt - temp_e) < 0.03) && (fabs(temp_e) < 0.1))
		{
			//Wenn Seitenweg gesehen
			if (Seitenweg_gesehen==true)
			{
				//auf Drehm鰃lichkeit pr黤en, wenn m鰃lich, dann Strategie "Turn"
				if ((Entf_rechts[0] > 2) && (Entf_links[0] > 2))
				{
					//stoppen
					Seitenweg_gesehen = false;
					temp_Entf_stop = Stop_vor_Wand;			//Stopentfernung wieder auf default setzen
					Handlung = Turn;		
				}
				//sonst wieder R點kw鋜ts und Seitengang ignorieren
				else
				{
					Seitenweg_gesehen = false;
					temp_Entf_stop = Stop_vor_Wand;	//Stop-Entfernung wieder auf default setzen
				}
			}
			//sonst Handlung Fwd, da Ruck nicht mehr m鰃lich
			else
			{
				temp_Entf_stop = Stop_vor_Wand;
				Handlung = Fwd;		
			}
		}

		//normaler R點kw鋜tsflug
		else
		{
			temp_e 			= Entf_hinten[0] - temp_Entf_stop; //Stopenffernung (evtl Seitenweg)
	//Leistung_x_soll ---------------------------------------------------------------
			temp_P 			= temp_e * P_Entf_Fwd;
			temp_I			= 0.5*(temp_e + e_Entf_Fwd_alt)*Prozesszyklus;
			temp_I 			= temp_I * I_Entf_Fwd + I_Anteil_Entf_Fwd;

			//Wenn e_Entfernung > 1m, keinen I_Anteil berechnen
			if (fabs(temp_e) > 1) temp_I = 0;			
			I_Anteil_Entf_Fwd= temp_I;

			temp_D	= (temp_e - e_Entf_Fwd_alt)/Prozesszyklus * D_Entf_Fwd;
			
			//Wenn e_Entfernung > 1m, keinen D_Anteil berechnen
			if (fabs(temp_e) > 1) temp_D = 0;
			
			e_Entf_Fwd_alt 	= temp_e;
			temp_Erg		= temp_P + temp_I + temp_D;
			
			if (temp_Erg >= 1){temp_Erg = 1;}
			if (temp_Erg <= -1){temp_Erg = -1;}
			
			//neuer Leistungssollwert (R點kw鋜ts)
			Leistung_x_soll = temp_Erg *(-1);	
	//-------------------------------------------------------------------------------

	//Richtung_soll -----------------------------------------------------------------

			//Regeln des Seitenabstandes auf 2m, bei einer Gangbreite von >4m, sonst mittig zum Gang
			
			//keine Regelung, wenn Entfernung > Reglebereich (R+L max. 6m)
			if (Entf_rechts[0] + Entf_links[0] >=4)
			{
				//Regelminimum: Entfernung < 3m
				if ((Entf_rechts[0] < 3) || (Entf_links[0] < 3))
				{
					//Regeln auf rechte Seite (2m)
					if (Entf_rechts[0] < Entf_links[0])
					{	
						//Seitenregelung false==Linksdrehung, wenn soll-ist==pos. auf 2m
						PID_Seite(false, Entf_rechts, Entf_soll);
					}

					//Regeln auf linke Seite (2m)
					else
					{
						//Seitenregelung true==Rechtsdrehung, wenn soll-ist==pos. auf 2m
						PID_Seite(true, Entf_links, Entf_soll);
					}
				}
			}
			//Regelung, wenn Entfernung < 4m auf Sollabstand (R+L)/2
			else
			{
				PID_Seite(false, Entf_rechts, (Entf_rechts[0]+Entf_links[0])/2);
			}
	//-----------------------------------------------------------------------------

		}//else normaler R點kw鋜tsflug
	}//lese Strategie ausf黨ren
}//Funktion


//*******************************************************************************
//Seitenregelung auf Seitenabstand_soll
void PID_Seite(bit Richtung, float Seitenabstand[5], float Seitenabstand_soll)
{
	//Richtung == false -> Rechtsdreh (negative Werte) wenn soll-ist==pos
	//Richtung == true -> Linksdreh (positive Werte) 
	float temp_P;
	float temp_I;
	float temp_D;
	float temp_e;
	float temp_Erg;
	float temp_x;
	
	temp_x = (Seitenabstand[0] - Seitenabstand[4])/5;
	temp_e 	= Seitenabstand_soll - Seitenabstand[0];//temp_x;

	//nur Regeln, wenn der Abstand sich <30cm ge鋘dert hat
	if (fabs(temp_x < 0.3))
	{
		temp_P 			= temp_e * P_Seite;
		temp_I			= 0.5*(temp_e + e_Seite_alt)*Prozesszyklus;
		temp_I 			= temp_I * I_Seite + I_Anteil_Seite;

		//Wenn abs(e_Seitenabstand) > 50cm, keinen I_Anteil berechnen
		if (fabs(temp_e) > 0.5) temp_I = 0;		
		I_Anteil_Seite	= temp_I;

		temp_D			= (temp_e - e_Seite_alt)/Prozesszyklus * D_Seite;

		e_Seite_alt 	= temp_e;
		temp_Erg		= temp_P + temp_I + temp_D;

		//Bestimmung der Richtung (s.o.)
		if (Richtung == false) temp_Erg = temp_Erg*(-1);
		
		//maximal zul鋝sige Richtungskorrektur ist 40