//Program.datamng.antrieb Program.datamng.antrieb = new Program.datamng.antrieb();
public double GetAngle(GPSpoint p1, GPSpoint p2)
{
double xDiff = p2.lat - p1.lat;
double yDiff = p2.lng - p1.lng;
return(Math.Atan2(yDiff, xDiff) * (180 / Math.PI));
}
public double DistanceTo(GPSpoint me, GPSpoint target, char unit = 'K')
{
try
{
if (me != null && target != null)
{
double rlat1 = Math.PI * me.lat / 180;
double rlat2 = Math.PI * target.lat / 180;
double theta = me.lng - target.lng;
double rtheta = Math.PI * theta / 180;
double dist =
Math.Sin(rlat1) * Math.Sin(rlat2) + Math.Cos(rlat1) *
Math.Cos(rlat2) * Math.Cos(rtheta);
dist = Math.Acos(dist);
dist = dist * 180 / Math.PI;
dist = dist * 60 * 1.1515;
switch (unit)
{
case 'K': //Kilometers -> default
return(dist * 1.609344);
case 'M': //Meters
return((dist * 1.609344) * 1000);
}
return(dist);
}
return(0);
}
catch (Exception ex)
{
Console.WriteLine();
Console.WriteLine("ERROR WHILE GETTING A DISTANCE");
Console.WriteLine(ex.Message);
Console.WriteLine();
return(0);
}
}
public void moves()
{
try
{
while (true)
{
while (Program.datamng.quopoint.lat == 0 && Program.datamng.quopoint.lng == 0)
{
Console.WriteLine("ERROR QOUPOINT IS NOT SET: " + Program.datamng.quopoint.lat + ";" + Program.datamng.quopoint.lng);
Thread.Sleep(2000);
}
while (DistanceTo(Program.datamng.quopoint, Program.datamng.targetpoint, 'M') >= Program.datamng.dis_approach)
{
Console.WriteLine(Convert.ToString(DistanceTo(Program.datamng.quopoint, Program.datamng.targetpoint, 'M')));
if (dodged)
{
Program.datamng.drive.stopp();
if (Program.datamng.qoupoint_changed.AddSeconds(30) <= DateTime.Now)
{
GPSpoint last_qp = Program.datamng.quopoint;
while (last_qp == Program.datamng.quopoint)
{
Console.WriteLine("ERROR QOUPOINT TO OLD: " + Program.datamng.quopoint.lat + ";" + Program.datamng.quopoint.lng);
Thread.Sleep(2000);
}
}
Console.WriteLine("AIMING...");
aim(Program.datamng.targetpoint);
Console.WriteLine("AIMING DONE");
if (Program.datamng.Sensoren[1].distance == Program.datamng.max_distance &&
Program.datamng.Sensoren[0].distance == Program.datamng.max_distance &&
Program.datamng.Sensoren[2].distance == Program.datamng.max_distance &&
Program.datamng.Sensoren[4].distance == Program.datamng.max_distance &&
Program.datamng.Sensoren[5].distance == Program.datamng.max_distance &&
Program.datamng.Sensoren[3].distance == Program.datamng.max_distance)
{
//Kein Hinderniss, weiterfahren
dodge_obsticle(0);
}
if (Program.datamng.Sensoren[1].distance <= 30 &&
Program.datamng.Sensoren[0].distance <= 10 &&
Program.datamng.Sensoren[2].distance <= 30 &&
Program.datamng.Sensoren[4].distance >= 30 &&
Program.datamng.Sensoren[5].distance >= 30 &&
Program.datamng.Sensoren[3].distance >= 30)
{
//Fronales Flächenhinderniss
dodge_obsticle(1);
}
if (Program.datamng.Sensoren[1].distance <= 15 &&
Program.datamng.Sensoren[0].distance <= 20 &&
Program.datamng.Sensoren[2].distance >= 30 &&
Program.datamng.Sensoren[4].distance >= 30 &&
Program.datamng.Sensoren[5].distance >= 30 &&
Program.datamng.Sensoren[3].distance >= 30)
{
//Fronal_Links Flächenhinderniss
dodge_obsticle(2);
}
if (Program.datamng.Sensoren[1].distance >= 30 &&
Program.datamng.Sensoren[0].distance <= 20 &&
Program.datamng.Sensoren[2].distance <= 15 &&
Program.datamng.Sensoren[4].distance >= 30 &&
Program.datamng.Sensoren[5].distance >= 30 &&
Program.datamng.Sensoren[3].distance >= 30)
{
//Fronal_Rechts Flächenhinderniss
dodge_obsticle(3);
}
if (Program.datamng.Sensoren[1].distance >= 30 &&
Program.datamng.Sensoren[0].distance >= 30 &&
Program.datamng.Sensoren[2].distance >= 30 &&
Program.datamng.Sensoren[4].distance <= 30 &&
Program.datamng.Sensoren[5].distance <= 30 &&
Program.datamng.Sensoren[3].distance <= 10)
{
//Hinten Flächenhinderniss
dodge_obsticle(4);
}
if (Program.datamng.Sensoren[1].distance >= 30 &&
Program.datamng.Sensoren[0].distance >= 30 &&
Program.datamng.Sensoren[2].distance <= 30 &&
Program.datamng.Sensoren[4].distance >= 30 &&
Program.datamng.Sensoren[5].distance <= 30 &&
Program.datamng.Sensoren[3].distance >= 30)
{
//Ganz Rechts Flächenhinderniss
dodge_obsticle(5);
}
if (Program.datamng.Sensoren[1].distance <= 30 &&
Program.datamng.Sensoren[0].distance >= 30 &&
Program.datamng.Sensoren[2].distance >= 30 &&
Program.datamng.Sensoren[4].distance <= 30 &&
Program.datamng.Sensoren[5].distance >= 30 &&
Program.datamng.Sensoren[3].distance >= 30)
{
//Ganz Links Flächenhinderniss
dodge_obsticle(6);
}
if (Program.datamng.Sensoren[1].distance <= 30 &&
Program.datamng.Sensoren[0].distance <= 30 &&
Program.datamng.Sensoren[2].distance >= 30 &&
Program.datamng.Sensoren[4].distance <= 30 &&
Program.datamng.Sensoren[5].distance >= 30 &&
Program.datamng.Sensoren[3].distance >= 30)
{
//Vorne_Links_Ecke
dodge_obsticle(7);
}
if (Program.datamng.Sensoren[1].distance >= 30 &&
Program.datamng.Sensoren[0].distance <= 30 &&
Program.datamng.Sensoren[2].distance <= 30 &&
Program.datamng.Sensoren[4].distance >= 30 &&
Program.datamng.Sensoren[5].distance <= 30 &&
Program.datamng.Sensoren[3].distance >= 30)
{
//Vorne_Rechts_Ecke
dodge_obsticle(8);
}
if (Program.datamng.Sensoren[1].distance <= 30 &&
Program.datamng.Sensoren[0].distance >= 30 &&
Program.datamng.Sensoren[2].distance >= 30 &&
Program.datamng.Sensoren[4].distance <= 30 &&
Program.datamng.Sensoren[5].distance >= 30 &&
Program.datamng.Sensoren[3].distance <= 30)
{
//Hinten_Links_Ecke
dodge_obsticle(9);
}
if (Program.datamng.Sensoren[1].distance >= 30 &&
Program.datamng.Sensoren[0].distance >= 30 &&
Program.datamng.Sensoren[2].distance <= 30 &&
Program.datamng.Sensoren[4].distance >= 30 &&
Program.datamng.Sensoren[5].distance <= 30 &&
Program.datamng.Sensoren[3].distance <= 30)
{
//Hinten_Rechts_Ecke
dodge_obsticle(10);
}
if (Program.datamng.Sensoren[1].distance >= 30 &&
Program.datamng.Sensoren[0].distance >= 30 &&
Program.datamng.Sensoren[2].distance >= 30 &&
Program.datamng.Sensoren[4].distance <= 15 &&
Program.datamng.Sensoren[5].distance >= 30 &&
Program.datamng.Sensoren[3].distance <= 20)
{
//Hinten_Links Flächenhinderniss
dodge_obsticle(11);
}
if (Program.datamng.Sensoren[1].distance >= 30 &&
Program.datamng.Sensoren[0].distance >= 30 &&
Program.datamng.Sensoren[2].distance >= 30 &&
Program.datamng.Sensoren[4].distance >= 30 &&
Program.datamng.Sensoren[5].distance <= 15 &&
Program.datamng.Sensoren[3].distance <= 20)
{
//Hinten_Rechts Flächenhinderniss
dodge_obsticle(12);
}
if (Program.datamng.Sensoren[1].distance <= 30 &&
Program.datamng.Sensoren[0].distance >= 30 &&
Program.datamng.Sensoren[2].distance <= 30 &&
Program.datamng.Sensoren[4].distance <= 30 &&
Program.datamng.Sensoren[5].distance <= 30 &&
Program.datamng.Sensoren[3].distance >= 30)
{
//Passage
dodge_obsticle(13);
}
if (Program.datamng.Sensoren[1].distance >= 30 &&
Program.datamng.Sensoren[0].distance >= 30 &&
Program.datamng.Sensoren[2].distance <= 10 &&
Program.datamng.Sensoren[4].distance >= 30 &&
Program.datamng.Sensoren[5].distance >= 30 &&
Program.datamng.Sensoren[3].distance >= 30)
{
//Punkt_Frontal_Rechts
dodge_obsticle(14);
}
if (Program.datamng.Sensoren[1].distance >= 30 &&
Program.datamng.Sensoren[0].distance <= 10 &&
Program.datamng.Sensoren[2].distance >= 30 &&
Program.datamng.Sensoren[4].distance >= 30 &&
Program.datamng.Sensoren[5].distance >= 30 &&
Program.datamng.Sensoren[3].distance >= 30)
{
//Punkt_Frontal
dodge_obsticle(15);
}
if (Program.datamng.Sensoren[1].distance >= 30 &&
Program.datamng.Sensoren[0].distance >= 30 &&
Program.datamng.Sensoren[2].distance >= 30 &&
Program.datamng.Sensoren[4].distance <= 10 &&
Program.datamng.Sensoren[5].distance >= 30 &&
Program.datamng.Sensoren[3].distance >= 30)
{
//Punkt_Frontal_Links
dodge_obsticle(16);
}
if (Program.datamng.Sensoren[1].distance >= 30 &&
Program.datamng.Sensoren[0].distance >= 30 &&
Program.datamng.Sensoren[2].distance >= 30 &&
Program.datamng.Sensoren[4].distance >= 30 &&
Program.datamng.Sensoren[5].distance <= 10 &&
Program.datamng.Sensoren[3].distance >= 30)
{
//Punkt_Frontal_Rechts
dodge_obsticle(17);
}
if (Program.datamng.Sensoren[1].distance >= 30 &&
Program.datamng.Sensoren[0].distance >= 30 &&
Program.datamng.Sensoren[2].distance >= 30 &&
Program.datamng.Sensoren[4].distance <= 10 &&
Program.datamng.Sensoren[5].distance >= 30 &&
Program.datamng.Sensoren[3].distance >= 30)
{
//Punkt_Frontal_Links
dodge_obsticle(18);
}
if (Program.datamng.Sensoren[1].distance >= 30 &&
Program.datamng.Sensoren[0].distance >= 30 &&
Program.datamng.Sensoren[2].distance >= 30 &&
Program.datamng.Sensoren[4].distance >= 30 &&
Program.datamng.Sensoren[5].distance >= 30 &&
Program.datamng.Sensoren[3].distance <= 10)
{
//Punkt_Hinten
dodge_obsticle(19);
}
}
Thread.Sleep(100);
}
Program.datamng.targetpoint = Program.datamng.route[Program.datamng.next_way_point];
Program.datamng.next_way_point++;
}
}
catch (Exception ex)
{
Console.WriteLine();
Console.WriteLine("ERROR WHILE MOVING");
Console.WriteLine(ex.Message);
Console.WriteLine();
}
}
void aim(GPSpoint target)
{
try
{
start_angle = GetAngle(Program.datamng.quopoint, target);
Console.WriteLine("START ANGLE: " + start_angle);
if (Program.datamng.Orientation - start_angle <= 180)
{//gegen uhrzeigersinn
angle_to_turn = Program.datamng.Orientation - start_angle;
while (Math.Abs(angle_to_turn) != Program.datamng.angle_approach)
{
angle_to_turn = Program.datamng.Orientation - start_angle;
Program.datamng.drive.turn_right();
Thread.Sleep(100);
Program.datamng.drive.stopp();
Thread.Sleep(100);
Console.WriteLine("ANGLE TO TURN: " + angle_to_turn);
if (target != Program.datamng.targetpoint)
{
Console.WriteLine("TARGET CHANGED!");
Console.WriteLine("OLD:" + target.lat + "," + target.lng);
target = Program.datamng.targetpoint;
Console.WriteLine("New:" + target.lat + "," + target.lng);
Console.WriteLine("OLD START ANGLE:" + start_angle);
start_angle = GetAngle(Program.datamng.quopoint, target);
Console.WriteLine("NEW START ANGLE:" + start_angle);
Console.WriteLine("OLD ANGLE TO TURN:" + angle_to_turn);
angle_to_turn = Program.datamng.Orientation - start_angle;
Console.WriteLine("OLD ANGLE TO TURN:" + angle_to_turn);
}
}
}
else
{//mit uhrzeigersinn
angle_to_turn = 360 - (Program.datamng.Orientation - start_angle);
while (Math.Abs(angle_to_turn) != Program.datamng.angle_approach)
{
angle_to_turn = 360 - (Program.datamng.Orientation - start_angle);
Program.datamng.drive.turn_left();
Thread.Sleep(100);
Program.datamng.drive.stopp();
Thread.Sleep(100);
Console.WriteLine("ANGLE TO TURN: " + angle_to_turn);
if (target != Program.datamng.targetpoint)
{
Console.WriteLine("TARGET CHANGED!");
Console.WriteLine("OLD POINT:" + target.lat + "," + target.lng);
target = Program.datamng.targetpoint;
Console.WriteLine("New POINT:" + target.lat + "," + target.lng);
Console.WriteLine("OLD START ANGLE:" + start_angle);
start_angle = GetAngle(Program.datamng.quopoint, target);
Console.WriteLine("NEW START ANGLE:" + start_angle);
Console.WriteLine("OLD ANGLE TO TURN:" + angle_to_turn);
angle_to_turn = 360 - (Program.datamng.Orientation - start_angle);
Console.WriteLine("OLD ANGLE TO TURN:" + angle_to_turn);
}
}
}
}
catch (Exception ex)
{
Console.WriteLine();
Console.WriteLine("ERROR WHILE AIMING: " + Convert.ToString(target.lat) + "," + Convert.ToString(target.lng));
Console.WriteLine(ex.Message);
Console.WriteLine();
}
}
