void OnTimer(object sender, EventArgs e)
{
for (int i = 0; i < Par.ITER_PER_TICK; i++)
{
RobotMgr.CalcNextPos();
}
MapMousePos();
RobotMgr.UpdatePath(); // Spur-Anzeige
RobotProg.SignalRobots(); // Robot-Progs von der Änderung benachrichtigen
this.Invalidate();
}
void OnMouseDown(object sender, MouseEventArgs e)
{
if (!createRobotsMenue.Checked)
{
label1.Text = RobotMgr.GetInfo(e.Location);
}
if (createObstaclesMenu.Checked)
{
Omgr.NewObstacle(e.Location);
}
else if (createRobotsMenue.Checked)
{
RobotMgr.CreateRobot(int.Parse(_prgNumEd.Text), e.Location);
}
this.Invalidate();
}
void ProgTrain()
{
Robot followingRobot = RobotMgr.GetNearest(rb);
while (true)
{
if (followingRobot != null)
{
SetFollow(followingRobot.Pos, 50, 0.1);
}
else
{
SetFollow(mpos, 0, 0.1);
}
WaitForUpdate();
}
}
void ProgDoge()
{
const double speed = 5;
Random ran = new Random();
DogeState state = DogeState.Turn;
double vFactor = 1;
double v = 0;
double turnedPhi = 0;
double relPhi = 0;
TurnAbsAngle(ran.Next(-180, 180), 5);
double previousPhi = GetAngleInRange(rb.GyroAngle);
while (true)
{
double phi = GetAngleInRange(rb.GyroAngle);
if (rb.Pos.X < DblBuffForm.XMin())
{
if (phi >= -180 && phi <= -90 && v > 0 && state != DogeState.BackwardClockwise)
{
state = DogeState.BackwardClockwise;
relPhi = 45;
turnedPhi = 0;
}
else if (phi >= 90 && phi <= 180 && v > 0 && state != DogeState.BackwardAnticlockwise)
{
state = DogeState.BackwardAnticlockwise;
relPhi = -45;
turnedPhi = 0;
}
else if (phi >= 0 && phi <= 90 && v < 0 && state != DogeState.ForwardClockwise)
{
state = DogeState.ForwardClockwise;
relPhi = 45;
turnedPhi = 0;
}
else if (phi >= -90 && phi <= 0 && v < 0 && state != DogeState.ForwardAnticlockwise)
{
state = DogeState.ForwardAnticlockwise;
relPhi = -45;
turnedPhi = 0;
}
}
else if (rb.Pos.X > DblBuffForm.XMax())
{
if (phi >= 0 && phi <= 90 && v > 0 && state != DogeState.BackwardClockwise)
{
state = DogeState.BackwardClockwise;
relPhi = 45;
turnedPhi = 0;
}
else if (phi >= -90 && phi <= 0 && v > 0 && state != DogeState.BackwardAnticlockwise)
{
state = DogeState.BackwardAnticlockwise;
relPhi = -45;
turnedPhi = 0;
}
else if (phi >= -180 && phi <= -90 && v < 0 && state != DogeState.ForwardClockwise)
{
state = DogeState.ForwardClockwise;
relPhi = 45;
turnedPhi = 0;
}
else if (phi >= 90 && phi <= 180 && v < 0 && state != DogeState.ForwardAnticlockwise)
{
state = DogeState.ForwardAnticlockwise;
relPhi = -45;
turnedPhi = 0;
}
}
else if (rb.Pos.Y < DblBuffForm.YMin())
{
if (phi >= -90 && phi <= 0 && v > 0 && state != DogeState.BackwardClockwise)
{
state = DogeState.BackwardClockwise;
relPhi = 45;
turnedPhi = 0;
}
else if (phi >= -180 && phi <= -90 && v > 0 && state != DogeState.BackwardAnticlockwise)
{
state = DogeState.BackwardAnticlockwise;
relPhi = -45;
turnedPhi = 0;
}
else if (phi >= 90 && phi <= 180 && v < 0 && state != DogeState.ForwardClockwise)
{
state = DogeState.ForwardClockwise;
relPhi = 45;
turnedPhi = 0;
}
else if (phi >= 0 && phi <= 90 && v < 0 && state != DogeState.ForwardAnticlockwise)
{
state = DogeState.ForwardAnticlockwise;
relPhi = -45;
turnedPhi = 0;
}
}
else if (rb.Pos.Y > DblBuffForm.YMax())
{
if (phi >= 90 && phi <= 180 && v > 0 && state != DogeState.BackwardClockwise)
{
state = DogeState.BackwardClockwise;
relPhi = 45;
turnedPhi = 0;
}
else if (phi >= 0 && phi <= 90 && v > 0 && state != DogeState.BackwardAnticlockwise)
{
state = DogeState.BackwardAnticlockwise;
relPhi = -45;
turnedPhi = 0;
}
else if (phi >= -90 && phi <= 0 && v < 0 && state != DogeState.ForwardClockwise)
{
state = DogeState.ForwardClockwise;
relPhi = 45;
turnedPhi = 0;
}
else if (phi >= -180 && phi <= -90 && v < 0 && state != DogeState.ForwardAnticlockwise)
{
state = DogeState.ForwardAnticlockwise;
relPhi = -45;
turnedPhi = 0;
}
}
else
{
Robot[] nearRobots = RobotMgr.GetRobots().Where(r => r != rb && DistanceTo(r) < 50).
OrderBy(r => DistanceTo(r)).ToArray();
int toNearRobotsCount = nearRobots.Count(r => DrivingTowardsOther(r));
if (toNearRobotsCount > 0)
{
state = DogeState.Turn;
double toRobotPhi = (nearRobots.First().Pos - rb.Pos).GetPhiGrad();
double relToRobotPhi = GetRelAngle(toRobotPhi);
relPhi = relToRobotPhi < 0 ? 180 : -180;
turnedPhi = 0;
}
else if (toNearRobotsCount == 0 && state == DogeState.Turn)
{
state = DogeState.Forward;
relPhi = 0;
turnedPhi = 0;
}
vFactor = nearRobots.Length > 0 ? Math.Pow(DistanceTo(nearRobots.First()) / 50, 3) : 1;
}
switch (state)
{
case DogeState.Forward:
rb.SetV(v = speed * vFactor);
break;
case DogeState.ForwardClockwise:
rb.SetV(v = speed * vFactor);
break;
case DogeState.ForwardAnticlockwise:
rb.SetV(v = speed * vFactor);
break;
case DogeState.BackwardClockwise:
rb.SetV(v = -speed * vFactor);
break;
case DogeState.BackwardAnticlockwise:
rb.SetV(v = -speed * vFactor);
break;
case DogeState.Turn:
rb.SetV(v = 0);
break;
}
if (SetTurnRelAngle(relPhi, 5, ref turnedPhi, ref previousPhi))
{
switch (state)
{
case DogeState.ForwardClockwise:
state = DogeState.Forward;
relPhi = 0;
turnedPhi = 0;
break;
case DogeState.ForwardAnticlockwise:
state = DogeState.Forward;
relPhi = 0;
turnedPhi = 0;
break;
case DogeState.BackwardClockwise:
state = DogeState.ForwardAnticlockwise;
relPhi = 45;
turnedPhi = 0;
break;
case DogeState.BackwardAnticlockwise:
state = DogeState.ForwardClockwise;
relPhi = -45;
turnedPhi = 0;
break;
case DogeState.Turn:
state = DogeState.Forward;
relPhi = 0;
turnedPhi = 0;
break;
}
}
WaitForUpdate();
}
}
void OnPaint(object sender, PaintEventArgs e)
{
RobotMgr.Paint(e.Graphics);
Omgr.Paint(e.Graphics);
}
protected override void OnFormClosing(FormClosingEventArgs e)
{
RobotMgr.CleanUp();
base.OnFormClosing(e);
}
