static void Main(string[] args)
{
Console.WriteLine("Lass wen retten");
//Test
Premises premises = new Premises(22, 20);
object[,] map = premises.generateMap();
RescueBot rescueBot = new RescueBot(premises.returnStartingPosition()[0], premises.returnStartingPosition()[1], premises);
// Start navigation oder so
// Bot ground movement Test // --> Works!!!
/*
* rescueBot.updateSurroundings();
* Console.WriteLine("PosY{0}, PosX{1}",rescueBot.positionY,rescueBot.positionX);
* rescueBot.driveForward();
* Console.WriteLine("Bot Pos : {0}", rescueBot.returnXpos());
* rescueBot.updateSurroundings();
* Console.WriteLine("PosY{0}, PosX{1}",rescueBot.positionY,rescueBot.positionX);
* rescueBot.driveForward();
* Console.WriteLine("Bot Pos : {0}", rescueBot.returnXpos());
* rescueBot.updateSurroundings();
* Console.WriteLine("PosY{0}, PosX{1}",rescueBot.positionY,rescueBot.positionX);
* rescueBot.driveForward();
* Console.WriteLine("Bot Pos : {0}", rescueBot.returnXpos());
* rescueBot.updateSurroundings();
* Console.WriteLine("PosY{0}, PosX{1}",rescueBot.positionY,rescueBot.positionX);
* rescueBot.driveForward();
* Console.WriteLine("Bot Pos : {0}", rescueBot.returnXpos());
* rescueBot.updateSurroundings();
* Console.WriteLine("PosY{0}, PosX{1}",rescueBot.positionY,rescueBot.positionX);
* rescueBot.driveForward();
* Console.WriteLine("Bot Pos : {0}", rescueBot.returnXpos());
* rescueBot.updateSurroundings();
* Console.WriteLine("PosY{0}, PosX{1}",rescueBot.positionY,rescueBot.positionX);
* rescueBot.driveForward();
*/
// TODO:
// Navigations algorithmus
// Automatisches aufsammeln von passenden gegenständen
// wenn sie sich links, rechts, vor oder hinter dem fahrzeug befinden
// entfernen der gegenstände auf der Karte DONE
// Bot auf der Karte anzeigen
// Bot auf der Karte bewegen
rescueBot.startNavigation(rescueBot);
}
public void startNavigation(RescueBot rescueBot)
{
int[] coords = new int[3];
coords = coordsToClosest(getSignal());
int destX = coords[1];
int destY = coords[2];
// destination muss aus getsignal und calc distance hervorgehen
navigation.startNavigation(rescueBot, destX, destY);
// Nachdem der erste Funkturm erreicht wurde zum zweiten navigieren
coords = coordsToClosest(getSignal());
destX = coords[1];
destY = coords[2];
navigation.startNavigation(rescueBot, destX, destY);
// Nachdem der zweite erreicht wurde zum dritten fahren
coords = coordsToClosest(getSignal());
destX = coords[1];
destY = coords[2];
navigation.startNavigation(rescueBot, destX, destY);
}
public bool startNavigation(RescueBot rescuebot, int destX, int destY)
{
// navigation
// zu erst muss geguckt werden in welche richtung gefahren werden soll.
// dazu sollte ständig eine differnez zwischen x und y wert berechnet werden
// ziel ist es die differenz auf 1 1 zu bringen
// Sollte der bot vor einem Hinderniss stehen, muss er sich einen anderen Weg suchen
// ---> objekt umfahren und weiter zu ziel
int destinationX = destX;
int destinationY = destY;
int differenzX = destinationX - rescuebot.returnXpos();
int differenzY = destinationY - rescuebot.returnYpos();
while (differenzX != 0 || differenzY != 0)
{
// Verlangsamen der einzelnen schritte zur besseren Viusalisierung
Thread.Sleep(200);
// BACKWARD //////////////////////////////////////////////////////////////////////////////
if (differenzY > 0)
{
rescuebot.updateSurroundings();
if (rescuebot.driveBackward())
{
Console.WriteLine("PosY{0}, PosX{1}", rescuebot.returnYpos(), rescuebot.returnXpos());
differenzY = destinationY - rescuebot.returnYpos();
}
else
{
do
{
rescuebot.driveRight();
rescuebot.updateSurroundings();
Console.WriteLine("PosY{0}, PosX{1}", rescuebot.returnYpos(), rescuebot.returnXpos());
differenzX = destinationX - rescuebot.returnXpos();
} while (rescuebot.driveBackward() == false);
}
if (differenzY == 1)
{
differenzY = 0;
}
}
// FORWARD //////////////////////////////////////////////////////////////////////////////
if (differenzY < 0)
{
rescuebot.updateSurroundings();
if (rescuebot.driveForward())
{
Console.WriteLine("PosY{0}, PosX{1}", rescuebot.returnYpos(), rescuebot.returnXpos());
differenzY = destinationY - rescuebot.returnYpos();
}
else
{
do
{
rescuebot.driveLeft();
rescuebot.updateSurroundings();
Console.WriteLine("PosY{0}, PosX{1}", rescuebot.returnYpos(), rescuebot.returnXpos());
differenzX = destinationX - rescuebot.returnXpos();
} while (rescuebot.driveForward() == false);
}
if (differenzY == -1)
{
differenzY = 0;
}
}
// RIGHT //////////////////////////////////////////////////////////////////////////////
if (differenzX > 0)
{
rescuebot.updateSurroundings();
if (rescuebot.driveRight())
{
Console.WriteLine("PosY{0}, PosX{1}", rescuebot.returnYpos(), rescuebot.returnXpos());
differenzX = destinationX - rescuebot.returnXpos();
}
else
{
do
{
rescuebot.driveForward();
rescuebot.updateSurroundings();
Console.WriteLine("PosY{0}, PosX{1}", rescuebot.returnYpos(), rescuebot.returnXpos());
differenzY = destinationY - rescuebot.returnYpos();
} while (rescuebot.driveRight() == false);
}
if (differenzX == 1)
{
differenzX = 0;
}
}
// LEFT //////////////////////////////////////////////////////////////////////////////
if (differenzX < 0)
{
rescuebot.updateSurroundings();
if (rescuebot.driveLeft())
{
Console.WriteLine("PosY{0}, PosX{1}", rescuebot.returnYpos(), rescuebot.returnXpos());
differenzX = destinationX - rescuebot.returnXpos();
}
else
{
do
{
rescuebot.driveBackward();
rescuebot.updateSurroundings();
Console.WriteLine("PosY{0}, PosX{1}", rescuebot.returnYpos(), rescuebot.returnXpos());
differenzY = destinationY - rescuebot.returnYpos();
} while (rescuebot.driveLeft() == false);
}
if (differenzX == -1)
{
differenzX = 0;
}
}
}
return(true);
}
