public void Block_Line_of_Sight(SSL_WrapperPacket packet, bool IsBlue)
{
if (IsBlue)
{
if (packet.detection.balls[0].x > -3025)
{
List<float> slopes = new List<float>();
IRobotInfo[] final_points = new IRobotInfo[3];
List<IRobotInfo> points_goal = new List<IRobotInfo>();
List<float> final_angles = new List<float>();
int point_with_small_y = 0, point_with_big_y = 2;
IRobotInfo tmp_point = new RobotParameters(-3025, 0, 0);
tmp_point.Y = -232;
points_goal.Add(Statics.DeepClone<IRobotInfo>(tmp_point));
tmp_point.Y = 0;
points_goal.Add(Statics.DeepClone<IRobotInfo>(tmp_point));
tmp_point.Y = 232;
points_goal.Add(Statics.DeepClone<IRobotInfo>(tmp_point));
slopes.Add((OwnGoal.Y - 232 - packet.detection.balls[0].y) / (OwnGoal.X - packet.detection.balls[0].x));
slopes.Add((OwnGoal.Y + 0 - packet.detection.balls[0].y) / (OwnGoal.X - packet.detection.balls[0].x));
slopes.Add((OwnGoal.Y + 232 - packet.detection.balls[0].y) / (OwnGoal.X - packet.detection.balls[0].x));
float x = 0, y = 0;
double ratio = 0.1666666666;
for (int i = 0; i < 3; i++)
{
if (float.IsInfinity(slopes[i]))
slopes[i] = 300;
if (i == 1)
{
while (Math.Sqrt(Math.Pow(x, 2) + Math.Pow(y, 2)) < 375)
{
if (slopes[i] > 9)
{
x = (float)(x + ratio);
y = (float)(y + ratio * slopes[i]);
}
else
{
x = x + 1;
y = y + slopes[i];
}
}
}
else
{
while (Math.Sqrt(Math.Pow(x, 2) + Math.Pow(y, 2)) < 800)
{
if (slopes[i] > 9)
{
x = (float)(x + ratio);
y = (float)(y + ratio * slopes[i]);
}
else
{
x = x + 1;
y = y + slopes[i];
}
}
}
tmp_point.X = points_goal[i].X + x;
tmp_point.Y = points_goal[i].Y + y;
final_points[i] = Common.Statics.DeepClone<IRobotInfo>(tmp_point);
x = 0;
y = 0;
}
final_angles.Add((float)Math.Atan2(OwnGoal.Y - 232 - packet.detection.balls[0].y, OwnGoal.X - packet.detection.balls[0].x));
final_angles.Add((float)Math.Atan2(OwnGoal.Y - 0 - packet.detection.balls[0].y, OwnGoal.X - packet.detection.balls[0].x));
final_angles.Add((float)Math.Atan2(OwnGoal.Y + 232 - packet.detection.balls[0].y, OwnGoal.X - packet.detection.balls[0].x));
for (int i = 0; i < 3; i++)
{
final_angles[i] += (float)Math.PI;
final_points[i].W = final_angles[i];
}
if (final_points[2].Y < final_points[0].Y)
{
point_with_small_y = 2;
point_with_big_y = 0;
}
if (packet.detection.robots_blue[OwnDefender1Id].y < packet.detection.robots_blue[OwnDefender2Id].y)
{
repo.OutData[OwnDefender1Id].X = final_points[point_with_small_y].X;
repo.OutData[OwnDefender1Id].Y = final_points[point_with_small_y].Y;
repo.OutData[OwnDefender1Id].W = final_points[point_with_small_y].W;
//DataHandler.WriteReference(final_points[point_with_small_y], Defender1_Blue);
repo.OutData[OwnGoalkeeperId].X = final_points[1].X;
repo.OutData[OwnGoalkeeperId].Y = final_points[1].Y;
repo.OutData[OwnGoalkeeperId].W = final_points[1].W;
//DataHandler.WriteReference(final_points[1], GoalKeeper_Blue_id);
repo.OutData[OwnDefender2Id].X = final_points[point_with_big_y].X;
repo.OutData[OwnDefender2Id].Y = final_points[point_with_big_y].Y;
repo.OutData[OwnDefender2Id].W = final_points[point_with_big_y].W;
//DataHandler.WriteReference(final_points[point_with_big_y], Defender2_Blue);
}
else
{
repo.OutData[OwnDefender1Id].X = final_points[point_with_big_y].X;
repo.OutData[OwnDefender1Id].Y = final_points[point_with_big_y].Y;
repo.OutData[OwnDefender1Id].W = final_points[point_with_big_y].W;
//DataHandler.WriteReference(final_points[point_with_big_y], Defender1_Blue);
repo.OutData[OwnGoalkeeperId].X = final_points[1].X;
repo.OutData[OwnGoalkeeperId].Y = final_points[1].Y;
repo.OutData[OwnGoalkeeperId].W = final_points[1].W;
//DataHandler.WriteReference(final_points[1], GoalKeeper_Blue_id);
repo.OutData[OwnDefender2Id].X = final_points[point_with_small_y].X;
repo.OutData[OwnDefender2Id].Y = final_points[point_with_small_y].Y;
repo.OutData[OwnDefender2Id].W = final_points[point_with_small_y].W;
//DataHandler.WriteReference(final_points[point_with_small_y], Defender2_Blue);
}
//if(Distance(packet.detection.robots_blue[Defender1_Blue].x,final_points[0].x,packet.detection.robots_blue[Defender1_Blue].y,final_points[0].y) < Distance(packet.detection.robots_blue[Defender2_Blue].x,final_points[0].x,
//rebuff = Defender1_Blue.ToString() + Environment.NewLine + final_points[0].x.ToString() + " " + final_points[0].y.ToString() + " " + final_points[0].w.ToString() +
// Environment.NewLine + GoalKeeper_Blue_id.ToString() + Environment.NewLine + final_points[1].x.ToString() + " " + final_points[1].y.ToString() + " " + final_points[1].w.ToString() +
// Environment.NewLine + Defender2_Blue.ToString() + Environment.NewLine + final_points[2].x.ToString() + " " + final_points[2].y.ToString() + " " + final_points[2].w.ToString();
}
}
else
{
if (packet.detection.balls[0].x < 3025)
{
List<float> slopes = new List<float>();
IRobotInfo[] final_points = new IRobotInfo[3];
List<IRobotInfo> points_goal = new List<IRobotInfo>();
List<float> final_angles = new List<float>();
int point_with_small_y = 0, point_with_big_y = 2;
IRobotInfo tmp_point = new RobotParameters(3025, 0, 0);
tmp_point.Y = -232;
points_goal.Add(Statics.DeepClone<IRobotInfo>(tmp_point));
tmp_point.Y = 0;
points_goal.Add(Statics.DeepClone<IRobotInfo>(tmp_point));
tmp_point.Y = 232;
points_goal.Add(Statics.DeepClone<IRobotInfo>(tmp_point));
slopes.Add((OppGoal.Y - 232 - packet.detection.balls[0].y) / (OppGoal.X - packet.detection.balls[0].x));
slopes.Add((OppGoal.Y + 0 - packet.detection.balls[0].y) / (OppGoal.X - packet.detection.balls[0].x));
slopes.Add((OppGoal.Y + 232 - packet.detection.balls[0].y) / (OppGoal.X - packet.detection.balls[0].x));
float x = 0, y = 0;
double ratio = 0.1666666666;
for (int i = 0; i < 3; i++)
{
if (float.IsInfinity(slopes[i]))
slopes[i] = 300;
if (i == 1)
{
while (Math.Sqrt(Math.Pow(x, 2) + Math.Pow(y, 2)) < 375)
{
if (slopes[i] > 9)
{
x = (float)(x - ratio);
y = (float)(y - ratio * slopes[i]);
}
else
{
x = x - 1;
y = y - slopes[i];
}
}
}
else
{
while (Math.Sqrt(Math.Pow(x, 2) + Math.Pow(y, 2)) < 800)
{
if (slopes[i] > 9)
{
x = (float)(x - ratio);
y = (float)(y - ratio * slopes[i]);
}
else
{
x = x - 1;
y = y - slopes[i];
}
}
}
tmp_point.X = points_goal[i].X + x;
tmp_point.Y = points_goal[i].Y + y;
final_points[i] = Statics.DeepClone<IRobotInfo>(tmp_point);
x = 0;
y = 0;
}
final_angles.Add((float)Math.Atan2(OppGoal.Y - 232 - packet.detection.balls[0].y, OppGoal.X - packet.detection.balls[0].x));
final_angles.Add((float)Math.Atan2(OppGoal.Y - 0 - packet.detection.balls[0].y, OppGoal.X - packet.detection.balls[0].x));
final_angles.Add((float)Math.Atan2(OppGoal.Y + 232 - packet.detection.balls[0].y, OppGoal.X - packet.detection.balls[0].x));
for (int i = 0; i < 3; i++)
{
final_angles[i] += (float)Math.PI;
final_points[i].W = final_angles[i];
}
if (final_points[2].Y < final_points[0].Y)
{
point_with_small_y = 2;
point_with_big_y = 0;
}
if (packet.detection.robots_yellow[OppDefender1Id].y < packet.detection.robots_yellow[OppDefender2Id].y)
{
repo.OutData[OppDefender1Id].X = final_points[point_with_small_y].X;
repo.OutData[OppDefender1Id].Y = final_points[point_with_small_y].Y;
repo.OutData[OppDefender1Id].W = final_points[point_with_small_y].W;
//DataHandler.WriteReference(final_points[point_with_small_y], Defender1_Yellow);
repo.OutData[OppGoalkeeperId].X = final_points[1].X;
repo.OutData[OppGoalkeeperId].Y = final_points[1].Y;
repo.OutData[OppGoalkeeperId].W = final_points[1].W;
//DataHandler.WriteReference(final_points[1], GoalKeeper_Yellow_id);
repo.OutData[OppDefender2Id].X = final_points[point_with_big_y].X;
repo.OutData[OppDefender2Id].Y = final_points[point_with_big_y].Y;
repo.OutData[OppDefender2Id].W = final_points[point_with_big_y].W;
//DataHandler.WriteReference(final_points[point_with_big_y], Defender2_Yellow);
}
else
{
repo.OutData[OppDefender1Id].X = final_points[point_with_big_y].X;
repo.OutData[OppDefender1Id].Y = final_points[point_with_big_y].Y;
repo.OutData[OppDefender1Id].W = final_points[point_with_big_y].W;
//DataHandler.WriteReference(final_points[point_with_big_y], Defender1_Yellow);
repo.OutData[OppGoalkeeperId].X = final_points[1].X;
repo.OutData[OppGoalkeeperId].Y = final_points[1].Y;
repo.OutData[OppGoalkeeperId].W = final_points[1].W;
//DataHandler.WriteReference(final_points[1], GoalKeeper_Yellow_id);
repo.OutData[OppDefender2Id].X = final_points[point_with_small_y].X;
repo.OutData[OppDefender2Id].Y = final_points[point_with_small_y].Y;
repo.OutData[OppDefender2Id].W = final_points[point_with_small_y].W;
//DataHandler.WriteReference(final_points[point_with_small_y], Defender2_Yellow);
}
}
//tb_private.Text += "Yellow Defender1 ID " + Defender1_Yellow + " and " + "Yellow Defender2 ID " + Defender2_Yellow + " blocking the goal " + "\r\n";
}
//Use Defender1_Yellow and Defender2_Support_Yellow for team 2 and Defender1_Blue and Defender2_Support_Blue for team 1.
}
/// <summary>
/// CMDragons Research Paper implementation for defence with 3 robots so that no one can shoot directly.
/// </summary>
/// <param name="IDG"> Own Goal Keeper ID.</param>
/// <param name="IDD1"> Own Defender 1 ID.</param>
/// <param name="IDD2"> Own Defender 2 ID.</param>
/// <param name="Goalkeeper"> Location of Own Goal keeper.</param>
/// <param name="Defender1"> Location of Own Defender 1.</param>
/// <param name="Defender2"> Location of Own Defender 2.</param>
/// <param name="ball"> Location of Ball.</param>
/// <param name="OwnGoal"> Location of Own Goal.</param>
public void BlockOppLineOfSight(int IDG, int IDD1, int IDD2, IRobotInfo Goalkeeper, IRobotInfo Defender1, IRobotInfo Defender2, SSL_DetectionBall ball, IRobotInfo OwnGoal)
{
List<float> slopes = new List<float>();
IRobotInfo[] final_points = new IRobotInfo[3];
List<IRobotInfo> points_goal = new List<IRobotInfo>();
List<float> final_angles = new List<float>();
int point_with_small_y = 0, point_with_big_y = 2;
points_goal.Add(new RobotParameters(OwnGoal.X, OwnGoal.Y - 232, 0));
points_goal.Add(OwnGoal);
points_goal.Add(new RobotParameters(OwnGoal.X, OwnGoal.Y + 232, 0));
IRobotInfo tmp_point = new RobotParameters(-3025, 0, 0);
//tmp_point.Y = -232;
//points_goal.Add(DataHandler.DeepClone<IRobotInfo>(tmp_point));
//tmp_point.Y = 0;
//points_goal.Add(DataHandler.DeepClone<IRobotInfo>(tmp_point));
//tmp_point.Y = 232;
//points_goal.Add(DataHandler.DeepClone<IRobotInfo>(tmp_point));
slopes.Add((OwnGoal.Y - 232 - ball.y) / (OwnGoal.X - ball.x));
slopes.Add((OwnGoal.Y + 0 - ball.y) / (OwnGoal.X - ball.x));
slopes.Add((OwnGoal.Y + 232 - ball.y) / (OwnGoal.X - ball.x));
float x = 0, y = 0;
float ratio = 0.1666666666f;
for (int i = 0; i < 3; i++)
{
if (float.IsInfinity(slopes[i]))
slopes[i] = 300;
if (i == 1)
{
while (Distance(x, 0, y, 0) < 375)
//while (Math.Sqrt(Math.Pow(x, 2) + Math.Pow(y, 2)) < 375)
{
if (slopes[i] > 9)
{
x = (float)(x + ratio);
y = (float)(y + ratio * slopes[i]);
}
else
{
x = x + 1;
y = y + slopes[i];
}
}
}
else
{
while (Distance(x, 0, y, 0) < 800)
//while (Math.Sqrt(Math.Pow(x, 2) + Math.Pow(y, 2)) < 800)
{
if (slopes[i] > 9)
{
x = (float)(x + ratio);
y = (float)(y + ratio * slopes[i]);
}
else
{
x = x + 1;
y = y + slopes[i];
}
}
}
tmp_point.X = points_goal[i].X + x;
tmp_point.Y = points_goal[i].Y + y;
final_points[i] = Statics.DeepClone<IRobotInfo>(tmp_point);
x = 0;
y = 0;
}
final_angles.Add((float)Math.Atan2(ball.y - (OwnGoal.Y - 232), ball.x - OwnGoal.X));
final_angles.Add((float)Math.Atan2(ball.y - (OwnGoal.Y - 0), ball.x - OwnGoal.X));
final_angles.Add((float)Math.Atan2(ball.y - (OwnGoal.Y + 232), ball.x - OwnGoal.X));
for (int i = 0; i < 3; i++)
{
final_points[i].W = final_angles[i];
}
if (final_points[2].Y < final_points[0].Y)
{
point_with_small_y = 2;
point_with_big_y = 0;
}
if (Defender1.Y < Defender2.Y)
{
repo.OutData[IDD1].X = final_points[point_with_small_y].X;
repo.OutData[IDD1].Y = final_points[point_with_small_y].Y;
repo.OutData[IDD1].W = final_points[point_with_small_y].W;
//DataHandler.WriteReference(final_points[point_with_small_y], IDD1);
repo.OutData[IDG].X = final_points[1].X; //Techincally this should be outside as it is same in if and else both
repo.OutData[IDG].Y = final_points[1].Y;
repo.OutData[IDG].W = final_points[1].W;
//DataHandler.WriteReference(final_points[1], IDG);
repo.OutData[IDD2].X = final_points[point_with_big_y].X;
repo.OutData[IDD2].Y = final_points[point_with_big_y].Y;
repo.OutData[IDD2].W = final_points[point_with_big_y].W;
//DataHandler.WriteReference(final_points[point_with_big_y], IDD2);
}
else
{
repo.OutData[IDD1].X = final_points[point_with_big_y].X;
repo.OutData[IDD1].Y = final_points[point_with_big_y].Y;
repo.OutData[IDD1].W = final_points[point_with_big_y].W;
//DataHandler.WriteReference(final_points[point_with_big_y], IDD1);
repo.OutData[IDG].X = final_points[1].X;
repo.OutData[IDG].Y = final_points[1].Y;
repo.OutData[IDG].W = final_points[1].W;
//DataHandler.WriteReference(final_points[1], IDG);
repo.OutData[IDD2].X = final_points[point_with_small_y].X;
repo.OutData[IDD2].Y = final_points[point_with_small_y].Y;
repo.OutData[IDD2].W = final_points[point_with_small_y].W;
//DataHandler.WriteReference(final_points[point_with_small_y], IDD2);
}
}
public void Block_Attacker(SSL_WrapperPacket packet, bool IsBlue)
{
IRobotInfo Final_Point_Marker = new RobotParameters(0, 0, 0);
float Final_Angle = 0;
float tmp_slope = 0;
if (IsBlue)
{
tmp_slope = (OwnGoal.Y - packet.detection.balls[0].y) / (OwnGoal.X - packet.detection.balls[0].x);
if (float.IsInfinity(tmp_slope))
tmp_slope = 300;
float x = 0;
float y = 0;
double ratio = 0.1666666666;
while (Math.Sqrt(Math.Pow(x, 2) + Math.Pow(y, 2)) < 300)
{
if (tmp_slope > 9)
{
x = (float)(x - ratio);
y = (float)(y - ratio * tmp_slope);
}
else
{
x = x - 1;
y = y - tmp_slope;
}
}
Final_Point_Marker.X = packet.detection.balls[0].x + x;
Final_Point_Marker.Y = packet.detection.balls[0].y + y;
Final_Angle = (float)Math.Atan2(OwnGoal.Y - packet.detection.balls[0].y, OwnGoal.X - packet.detection.balls[0].x);
Final_Angle = (float)(Final_Angle + Math.PI);
Final_Point_Marker.W = Final_Angle;
repo.OutData[OwnAttackerId].X = Final_Point_Marker.X;
repo.OutData[OwnAttackerId].Y = Final_Point_Marker.Y;
repo.OutData[OwnAttackerId].W = Final_Point_Marker.W;
//DataHandler.WriteReference(Final_Point_Marker, Attacker_Blue);
}
else
{
tmp_slope = (OppGoal.Y - packet.detection.balls[0].y) / (OppGoal.X - packet.detection.balls[0].x);
if (float.IsInfinity(tmp_slope))
tmp_slope = 300;
float x = 0;
float y = 0;
double ratio = 0.16666666666;
while (Math.Sqrt(Math.Pow(x, 2) + Math.Pow(y, 2)) < 300)
{
if (tmp_slope > 9)
{
x = (float)(x + ratio);
y = (float)(y + ratio * tmp_slope);
}
else
{
x = x + 1;
y = y + tmp_slope;
}
}
Final_Point_Marker.X = packet.detection.balls[0].x + x;
Final_Point_Marker.Y = packet.detection.balls[0].y + y;
Final_Angle = (float)Math.Atan2(OppGoal.Y - packet.detection.balls[0].y, OppGoal.X - packet.detection.balls[0].x);
Final_Angle = (float)(Final_Angle + Math.PI);
Final_Point_Marker.W = Final_Angle;
repo.OutData[OppAttackerId].X = Final_Point_Marker.X;
repo.OutData[OppAttackerId].Y = Final_Point_Marker.Y;
repo.OutData[OppAttackerId].W = Final_Point_Marker.W;
//DataHandler.WriteReference(Final_Point_Marker, Attacker_Yellow);
//Yellow Function Goes Here
}
//Use Marker_Yellow for team 2 and Marker_Blue for team 1.
}
/// <summary>
/// Used to move a robot in a circle of fixed radius.
/// </summary>
/// <param name="ID"> Id of robot to be moved.</param>
/// <param name="source"> Location of Robot to be moved.</param>
/// <param name="center"> The center point of the circle.</param>
/// <param name="target"> the point , when robot is facing will stop to move in circle.</param>
/// <param name="radius"> The Radius of the circle to move in.</param>
/// <returns>returns true when robot is facing the target otherwise returns false.</returns>
public bool RotateArroundPoint(int ID, IRobotInfo source, IRobotInfo center, IRobotInfo target, float radius)
{
float finalangle = (float)Math.Atan2(center.Y - target.Y, center.X - target.X);
IRobotInfo rb1 = new RobotParameters();
float currentangle = (float)Math.Atan2(source.Y - center.Y, source.X - center.X);
float Offset = 1.30f; // 75degrees
//if (Math.Abs(finalangle - currentangle) > Math.PI)
// Offset = -Offset;
if (((finalangle) - (currentangle)) > (Offset))
{
currentangle += Offset;
rb1.X = center.X + radius * (float)Math.Cos(currentangle);
rb1.Y = center.Y + radius * (float)Math.Sin(currentangle);
repo.OutData[ID].X = rb1.X;
repo.OutData[ID].Y = rb1.Y;
repo.OutData[ID].W = (float)Math.Atan2(target.Y - center.Y, target.X - center.X);
//DataHandler.WriteReference(new IRobotInfo(rb1.X, rb1.Y, (float)Math.Atan2(target.y - center.y, target.x - center.x)), ID);
return false;
}
else if (((finalangle) - (currentangle)) > (0.05f))
{
rb1.X = center.X + radius * (float)Math.Cos(finalangle);
rb1.Y = center.Y + radius * (float)Math.Sin(finalangle);
repo.OutData[ID].X = rb1.X;
repo.OutData[ID].Y = rb1.Y;
repo.OutData[ID].W = (float)Math.Atan2(target.Y - center.Y, target.X - center.X);
//DataHandler.WriteReference(new IRobotInfo(rb1.X, rb1.Y, (float)Math.Atan2(target.y - center.y, target.x - center.x)), ID);
return false;
}
else
{ return true; }
}
public IRobotInfo[] TrianglePoints(SSL_WrapperPacket packet, IRobotInfo source, int TargetID, bool IsBlue)
{
IRobotInfo[] t = new IRobotInfo[2];
float angle = (float)Math.Atan2(source.Y - packet.detection.robots_blue[TargetID].y, source.X - packet.detection.robots_blue[TargetID].x);
t[0] = new RobotParameters((float)(-100 * Math.Sin(angle)) + source.X, (float)(100 * Math.Cos(angle)) + source.Y, 0);
t[1] = new RobotParameters((float)(100 * Math.Sin(angle)) + source.X, (float)(-100 * Math.Cos(angle)) + source.Y, 0);
return t;
}
public IRobotInfo[] PlanExclusive(SSL_WrapperPacket mainPacket)
{
for (int i = 0; i < _gamePads.Length; i++)
{
if (_gamePads[i].Acquire().IsFailure)
continue;
if (_gamePads[i].Poll().IsFailure)
continue;
_states[i] = _gamePads[i].GetCurrentState();
if (Result.Last.IsFailure)
continue;
else
{
IRobotInfo robotInfo = new RobotParameters();
robotInfo.Id = 1;
//Get Current Robot Position
//Set Next Robot Position By adding the GamePad Values in Current Positon
// example
//currVAlX += _states[i].X;
//currVALY += _states[i].Y;
//currVALZ += _states[i].Z;
//Call GrabBall Function from AI AIPlanner if you want (To Rotate and Go to that point)
//Call GotoBall Function from AI AIPlanner if you want (TO Goto that point with Same direction)
//Send Value to PID;
//Write values to _repReference.OutData
return new[] {robotInfo};
}
}
return null;
}
public void rotateabout(SSL_WrapperPacket packet, IRobotInfo target, float conf)
{
//packet.detection.robots_blue[Attacker_Blue]
//PointF[] paray=new PointF[360];
//for (int i=0;i<360;i++)
float finalangle = (float)Math.Atan2(packet.detection.balls[0].y - target.Y, packet.detection.balls[0].x - target.X);
IRobotInfo rb1 = new RobotParameters();
float i = (float)Math.Atan2(packet.detection.robots_blue[OwnAttackerId].y - packet.detection.balls[0].y, packet.detection.robots_blue[OwnAttackerId].x - packet.detection.balls[0].x);
float Offset = 1.30f;//0.69808027923211169284467713787086f;
if (Math.Abs(finalangle - i) > Math.PI)
Offset = -Offset;
if (finalangle - i > Offset)//0.69808027923211169284467713787086f)
{
//rebuff = (finalangle * 57.3).ToString() + " x " + (i * 57.3).ToString();
i += Offset;
rb1.X = packet.detection.balls[0].x + conf * (float)Math.Cos(i);
rb1.Y = packet.detection.balls[0].y + conf * (float)Math.Sin(i);
//rb1.w = (float)Math.Atan2(packet.detection.balls[0].y - packet.detection.robots_blue[Attacker_Blue].y, packet.detection.balls[0].x - packet.detection.robots_blue[Attacker_Blue].x);
//rb1.w += (float)Offset;
repo.OutData[OwnAttackerId].X = rb1.X;
repo.OutData[OwnAttackerId].Y = rb1.Y;
repo.OutData[OwnAttackerId].W = (float)Math.Atan2(0 - repo.InData.GetBalls(0)[0].y, 3025 - repo.InData.GetBalls(0)[0].x);
//DataHandler.WriteReference(new RobotParameters(rb1.X, rb1.Y, (float)Math.Atan2(0 - packet.detection.balls[0].y, 3025 - packet.detection.balls[0].x)), Attacker_Blue);
}
else if (finalangle - i > 0.05f)
{
//rebuff = (finalangle * 57.3).ToString() + Environment.NewLine;
rb1.X = packet.detection.balls[0].x + conf * (float)Math.Cos(finalangle);
rb1.Y = packet.detection.balls[0].y + conf * (float)Math.Sin(finalangle);
//rb1.w = (float)Math.Atan2(packet.detection.balls[0].y - packet.detection.robots_blue[Attacker_Blue].y, packet.detection.balls[0].x - packet.detection.robots_blue[Attacker_Blue].x);
//rb1.w += (float)(40 / 57.3);
repo.OutData[OwnAttackerId].X = rb1.X;
repo.OutData[OwnAttackerId].Y = rb1.Y;
repo.OutData[OwnAttackerId].X = (float)Math.Atan2(0 - packet.detection.balls[0].y, 3025 - packet.detection.balls[0].x);
//DataHandler.WriteReference(new IRobotInfo(rb1.X, rb1.Y, (float)Math.Atan2(0 - packet.detection.balls[0].y, 3025 - packet.detection.balls[0].x)), Attacker_Blue);
}
else
GrabOrKick(packet, OwnAttackerId);
}
/// <summary>
/// Used to send two robots i.e. AttackSupport and FreeRole to make space a head for pass/Attack.
/// </summary>
/// <param name="IDAS"> Attack Support Robot Id.</param>
/// <param name="IDFR"> Free Role Robot Id.</param>
/// <param name="AttackSupport"> Attack Support Robot Position.</param>
/// <param name="FreeRole"> Free Role Robot Position.</param>
/// <param name="ball"> Ball Postion.</param>
/// <param name="OppMean"> Mean of Opponents All Robots.</param>
/// <param name="OppSD"> Standard Deviation of Opponents All Robots.</param>
/// <returns> Returns true if function Executes Successfully.</returns>
public bool MakeFreeSpace(int IDAS, int IDFR, IRobotInfo AttackSupport, IRobotInfo FreeRole, SSL_DetectionBall ball, PointF OppMean, PointF OppSD)
{
IRobotInfo Final_Point_Support = new RobotParameters();
IRobotInfo Final_Point_Mid = new RobotParameters();
if (OppMean.Y > 0)
{
if (Distance(AttackSupport.X, OppMean.X, AttackSupport.Y, OppMean.Y) < Distance(FreeRole.X, OppMean.X, FreeRole.Y, OppMean.Y))
{
Final_Point_Support.X = OppMean.X;
Final_Point_Support.Y = OppMean.Y;
Final_Point_Mid.X = OppMean.X;
Final_Point_Mid.Y = -9 * OppSD.Y / 5;
}
else
{
Final_Point_Support.X = OppMean.X;
Final_Point_Support.Y = -9 * OppSD.Y / 5;
Final_Point_Mid.X = OppMean.X;
Final_Point_Mid.Y = OppMean.Y;
}
}
else
{
if (Distance(AttackSupport.X, OppMean.X, AttackSupport.Y, -OppMean.Y) < Distance(FreeRole.X, OppMean.X, FreeRole.Y, -OppMean.Y))
{
Final_Point_Support.X = OppMean.X;
Final_Point_Support.Y = -OppMean.Y;
Final_Point_Mid.X = OppMean.X;
Final_Point_Mid.Y = 9 * OppSD.Y / 5;
}
else
{
Final_Point_Support.X = OppMean.X;
Final_Point_Support.Y = 9 * OppSD.Y / 5;
Final_Point_Mid.X = OppMean.X;
Final_Point_Mid.Y = -OppMean.Y;
}
}
Final_Point_Support.W = (float)Math.Atan2(ball.y - Final_Point_Support.Y, ball.x - Final_Point_Support.X);
Final_Point_Mid.W = (float)Math.Atan2(ball.y - Final_Point_Mid.Y, ball.x - Final_Point_Mid.X);
if (Distance(AttackSupport.X, Final_Point_Support.X, AttackSupport.Y, Final_Point_Support.Y) > 100 || Distance(FreeRole.X, Final_Point_Mid.X, FreeRole.Y, Final_Point_Mid.Y) > 100)
{
repo.OutData[IDAS].X = Final_Point_Support.X;
repo.OutData[IDAS].Y = Final_Point_Support.Y;
repo.OutData[IDAS].W = Final_Point_Support.W;
//DataHandler.WriteReference(Final_Point_Support, IDAS);
repo.OutData[IDFR].X = Final_Point_Mid.X;
repo.OutData[IDFR].Y = Final_Point_Mid.Y;
repo.OutData[IDFR].W = Final_Point_Mid.W;
//DataHandler.WriteReference(Final_Point_Mid, IDFR);
return false;
}
else
return true;
}
public void Mark_Opp_Attack_Support(SSL_WrapperPacket packet, bool IsBlue)
{
IRobotInfo Final_Point_Marker = new RobotParameters(0, 0, 0);
float Final_Angle = 0;
float tmp_slope = 0;
if (IsBlue)
{
tmp_slope = (packet.detection.robots_yellow[OppAttackSupportId].y - packet.detection.robots_yellow[OppAttackerId].y) / (packet.detection.robots_yellow[OppAttackSupportId].x - packet.detection.robots_yellow[OppAttackerId].x);
if (float.IsInfinity(tmp_slope))
tmp_slope = 300;
float x = 0;
float y = 0;
double ratio = 0.1666666;
while (Math.Sqrt(Math.Pow(x, 2) + Math.Pow(y, 2)) < 300)
{
if (packet.detection.robots_yellow[OppAttackSupportId].x <= packet.detection.robots_yellow[OppAttackerId].x)
{
if (tmp_slope > 9)
{
x = (float)(x + ratio);
y = (float)(y + ratio * tmp_slope);
}
else
{
x = x + 1;
y = y + tmp_slope;
}
}
else
{
if (tmp_slope > 9)
{
x = (float)(x - ratio);
y = (float)(y - ratio * tmp_slope);
}
else
{
x = x - 1;
y = y - tmp_slope;
}
}
}
Final_Point_Marker.X = packet.detection.robots_yellow[OppAttackSupportId].x + x;
Final_Point_Marker.Y = packet.detection.robots_yellow[OppAttackSupportId].y + y;
Final_Angle = (float)Math.Atan2(packet.detection.robots_yellow[OppAttackSupportId].y - packet.detection.robots_yellow[OppAttackerId].y, packet.detection.robots_yellow[OppAttackSupportId].x - packet.detection.robots_yellow[OppAttackerId].x);
Final_Angle = (float)(Final_Angle + Math.PI);
Final_Point_Marker.W = Final_Angle;
//MessageBox.Show(Final_Angle.ToString());
repo.OutData[OwnAttackSupportId].X = Final_Point_Marker.X;
repo.OutData[OwnAttackSupportId].Y = Final_Point_Marker.Y;
repo.OutData[OwnAttackSupportId].W = Final_Point_Marker.W;
//DataHandler.WriteReference(Final_Point_Marker, Attack_Supporter_Blue);
//tb_private.Text += "Blue Marker ID " + Marker_Blue + " blocking goal " + "\r\n";
//tb_private.Text += "Blue Midfielder ID " + Free_Role_Blue + " moves to (" + Final_Point_Marker.X + ", " + Final_Point_Marker.Y + ")" + "\r\n";
}
else
{
tmp_slope = (packet.detection.robots_blue[OwnAttackSupportId].y - packet.detection.robots_blue[OwnAttackerId].y) / (packet.detection.robots_blue[OwnAttackSupportId].x - packet.detection.robots_blue[OwnAttackerId].x);
//.Show(tmp_slope.ToString());
if (float.IsInfinity(tmp_slope))
tmp_slope = 300;
//MessageBox.Show(tmp_slope.ToString());
float x = 0;
float y = 0;
double ratio = 0.1666666666666;
while (Math.Sqrt(Math.Pow(x, 2) + Math.Pow(y, 2)) < 300)
{
if (packet.detection.robots_blue[OwnAttackSupportId].x <= packet.detection.robots_blue[OwnAttackerId].x)
{
if (tmp_slope > 9)
{
x = (float)(x + ratio);
y = (float)(y + ratio * tmp_slope);
}
else
{
x = x + 1;
y = y + tmp_slope;
}
}
else
{
if (tmp_slope > 9)
{
x = (float)(x - ratio);
y = (float)(y - ratio * tmp_slope);
}
else
{
x = x - 1;
y = y - tmp_slope;
}
//MessageBox.Show(x.ToString() + ", " + y.ToString());
}
}
Final_Point_Marker.X = packet.detection.robots_blue[OwnAttackSupportId].x + x;
Final_Point_Marker.Y = packet.detection.robots_blue[OwnAttackSupportId].y + y;
Final_Angle = (float)Math.Atan2(packet.detection.robots_blue[OwnAttackSupportId].y - packet.detection.robots_blue[OwnAttackerId].y, packet.detection.robots_blue[OwnAttackSupportId].x - packet.detection.robots_blue[OwnAttackerId].x);
Final_Angle = (float)(Final_Angle + Math.PI);
Final_Point_Marker.W = Final_Angle;
//MessageBox.Show(Final_Angle.ToString());
repo.OutData[OppAttackSupportId].X = Final_Point_Marker.X;
repo.OutData[OppAttackSupportId].Y = Final_Point_Marker.Y;
repo.OutData[OppAttackSupportId].W = Final_Point_Marker.W;
//DataHandler.WriteReference(Final_Point_Marker, Attack_Supporter_Yellow);
//tb_private.Text += "Yellow team's Midfielder ID " + Free_Role_Blue + " marking Blue's Attack Supporter ID " + Attack_Supporter_Blue + "\r\n";
}
}
public void GrabBall(SSL_WrapperPacket packet, bool IsBlue)
{
if (IsBlue)
{
IRobotInfo final = new RobotParameters(packet.detection.balls[0].x, packet.detection.balls[0].y,
(float)Math.Atan2(packet.detection.balls[0].y - packet.detection.robots_blue[OwnAttackerId].y,
packet.detection.balls[0].x - packet.detection.robots_blue[OwnAttackerId].x));
repo.OutData[OwnAttackerId].X = final.X;
repo.OutData[OwnAttackerId].X = final.Y;
repo.OutData[OwnAttackerId].X = final.W;
//DataHandler.WriteReference(final, Attacker_Blue);
repo.OutData[OwnAttackerId].Grab = true;
//DataHandler.Dribble[Attacker_Blue] = true;
}
else
{
IRobotInfo final = new RobotParameters(packet.detection.balls[0].x, packet.detection.balls[0].y,
(float)Math.Atan2(packet.detection.balls[0].y - packet.detection.robots_yellow[OppAttackerId].y,
packet.detection.balls[0].x - packet.detection.robots_yellow[OppAttackerId].x));
repo.OutData[OppAttackerId].X = final.X;
repo.OutData[OppAttackerId].X = final.Y;
repo.OutData[OppAttackerId].X = final.W;
//DataHandler.WriteReference(final, Attacker_Yellow);
repo.OutData[OppAttackerId].Grab = true;
//DataHandler.Dribble[Attacker_Yellow] = true;
}
}
public void MakeFreeSpace(SSL_WrapperPacket packet, bool IsBlue)
{
IRobotInfo Final_Point_Support = new RobotParameters();
IRobotInfo Final_Point_Mid = new RobotParameters();
if (IsBlue)
{
//tb_private.Text += "Blue team's Attack Support ID " + Attack_Supporter_Blue + " and Midfielder ID " + Free_Role_Blue + " move into the free space" + "\r\n";
if (OppRobotsMean.Y > 0)
{
if (Distance(packet.detection.robots_blue[OwnAttackSupportId].x, OppRobotsMean.X, packet.detection.robots_blue[OwnAttackSupportId].y, OppRobotsMean.Y) < Distance(packet.detection.robots_blue[OwnFreeRoleId].x, OppRobotsMean.X, packet.detection.robots_blue[OwnFreeRoleId].y, OppRobotsMean.Y))
{
Final_Point_Support.X = OppRobotsMean.X;
Final_Point_Support.Y = OppRobotsMean.Y;
Final_Point_Mid.X = OppRobotsMean.X;
Final_Point_Mid.Y = -9 * OppRobotsStandardDeviation.Y / 5;
}
else
{
Final_Point_Support.X = OppRobotsMean.X;
Final_Point_Support.Y = -9 * OppRobotsStandardDeviation.Y / 5;
Final_Point_Mid.X = OppRobotsMean.X;
Final_Point_Mid.Y = OppRobotsMean.Y;
}
}
else
{
if (Distance(packet.detection.robots_blue[OwnAttackSupportId].x, OppRobotsMean.X, packet.detection.robots_blue[OwnAttackSupportId].y, -OppRobotsMean.Y) < Distance(packet.detection.robots_blue[OwnFreeRoleId].x, OppRobotsMean.X, packet.detection.robots_blue[OwnFreeRoleId].y, -OppRobotsMean.Y))
{
Final_Point_Support.X = OppRobotsMean.X;
Final_Point_Support.Y = -OppRobotsMean.Y;
Final_Point_Mid.X = OppRobotsMean.X;
Final_Point_Mid.Y = 9 * OppRobotsStandardDeviation.Y / 5;
}
else
{
Final_Point_Support.X = OppRobotsMean.X;
Final_Point_Support.Y = 9 * OppRobotsStandardDeviation.Y / 5;
Final_Point_Mid.X = OppRobotsMean.X;
Final_Point_Mid.Y = -OppRobotsMean.Y;
}
}
Final_Point_Support.W = (float)Math.Atan2(packet.detection.balls[0].y - Final_Point_Support.Y, packet.detection.balls[0].x - Final_Point_Support.X);
Final_Point_Mid.W = (float)Math.Atan2(packet.detection.balls[0].y - Final_Point_Mid.Y, packet.detection.balls[0].x - Final_Point_Mid.X);
repo.OutData[OwnAttackSupportId].X = Final_Point_Support.X;
repo.OutData[OwnAttackSupportId].Y = Final_Point_Support.Y;
repo.OutData[OwnAttackSupportId].W = Final_Point_Support.W;
//DataHandler.WriteReference(Final_Point_Support, Attack_Supporter_Blue);
repo.OutData[OwnFreeRoleId].X = Final_Point_Mid.X;
repo.OutData[OwnFreeRoleId].Y = Final_Point_Mid.Y;
repo.OutData[OwnFreeRoleId].W = Final_Point_Mid.W;
//DataHandler.WriteReference(Final_Point_Mid, Free_Role_Blue);
//tb_private.Text += "Blue team's Attack Support ID " + Attack_Supporter_Blue + " moves to (" + Final_Point_Support.X + ", " + Final_Point_Support.Y + ") and Midfielder ID " + Free_Role_Blue + " moves to (" + Final_Point_Mid.X + ", " + Final_Point_Mid.Y + ")" + "\r\n";
}
else
{
if (OwnRobotsMean.Y > 0)
{
if (Distance(packet.detection.robots_yellow[OppAttackSupportId].x, OwnRobotsMean.X, packet.detection.robots_yellow[OppAttackSupportId].y, OwnRobotsMean.Y) < Distance(packet.detection.robots_yellow[OppFreeRoleId].x, OwnRobotsMean.X, packet.detection.robots_yellow[OppFreeRoleId].y, OwnRobotsMean.Y))
{
Final_Point_Support.X = OwnRobotsMean.X;
Final_Point_Support.Y = OwnRobotsMean.Y;
Final_Point_Mid.X = OwnRobotsMean.X;
Final_Point_Mid.Y = -9 * OwnRobotsStandardDeviation.Y / 5;
}
else
{
Final_Point_Support.X = OwnRobotsMean.X;
Final_Point_Support.Y = -9 * OwnRobotsStandardDeviation.Y / 5;
Final_Point_Mid.X = OwnRobotsMean.X;
Final_Point_Mid.Y = OwnRobotsMean.Y;
}
}
else
{
if (Distance(packet.detection.robots_yellow[OppAttackSupportId].x, OwnRobotsMean.X, packet.detection.robots_yellow[OppAttackSupportId].y, -OwnRobotsMean.Y) < Distance(packet.detection.robots_yellow[OppFreeRoleId].x, OwnRobotsMean.X, packet.detection.robots_yellow[OppFreeRoleId].y, -OwnRobotsMean.Y))
{
Final_Point_Support.X = OwnRobotsMean.X;
Final_Point_Support.Y = -OwnRobotsMean.Y;
Final_Point_Mid.X = OwnRobotsMean.X;
Final_Point_Mid.Y = 9 * OwnRobotsStandardDeviation.Y / 5;
}
else
{
Final_Point_Support.X = OwnRobotsMean.X;
Final_Point_Support.Y = 9 * OwnRobotsStandardDeviation.Y / 5;
Final_Point_Mid.X = OwnRobotsMean.X;
Final_Point_Mid.Y = -OwnRobotsMean.Y;
}
}
Final_Point_Mid.W = (float)Math.Atan2(packet.detection.balls[0].y - Final_Point_Mid.Y, packet.detection.balls[0].x - Final_Point_Mid.X);
Final_Point_Support.W = (float)Math.Atan2(packet.detection.balls[0].y - Final_Point_Support.Y, packet.detection.balls[0].x - Final_Point_Support.X);
repo.OutData[OppAttackSupportId].X = Final_Point_Support.X;
repo.OutData[OppAttackSupportId].Y = Final_Point_Support.Y;
repo.OutData[OppAttackSupportId].W = Final_Point_Support.W;
//DataHandler.WriteReference(Final_Point_Support, Attack_Supporter_Yellow);
repo.OutData[OppFreeRoleId].X = Final_Point_Mid.X;
repo.OutData[OppFreeRoleId].Y = Final_Point_Mid.Y;
repo.OutData[OppFreeRoleId].W = Final_Point_Mid.W;
//DataHandler.WriteReference(Final_Point_Mid, Free_Role_Yellow);
//MessageBox.Show(Final_Point_Support.x.ToString() + ", " + Final_Point_Support.y.ToString() + " " + Final_Point_Mid.x.ToString() + ", " + Final_Point_Mid.y.ToString());
//tb_private.Text += "Yellow team's Attack Support ID " + Attack_Supporter_Yellow + " and Midfielder ID " + Free_Role_Yellow + " move into the free space" + "\r\n";
}
}
public void GoNearBall(SSL_WrapperPacket packet, bool IsBlue, float conf)
{
if (IsBlue)
{
float dis = Distance(packet.detection.robots_blue[OwnAttackerId].x, packet.detection.balls[0].x, packet.detection.robots_blue[OwnAttackerId].y, packet.detection.balls[0].y);
float angle = (float)Math.Atan2(packet.detection.balls[0].y - packet.detection.robots_blue[OwnAttackerId].y, packet.detection.balls[0].x - packet.detection.robots_blue[OwnAttackerId].x);
dis -= conf;
IRobotInfo Target = new RobotParameters(packet.detection.robots_blue[OwnAttackerId].x + (float)(dis * Math.Cos(angle)), packet.detection.robots_blue[OwnAttackerId].y + (float)(dis * Math.Sin(angle)), (float)Math.Atan2(0 - packet.detection.balls[0].y, 3025 - packet.detection.balls[0].x));
repo.OutData[OwnAttackerId].X = Target.X;
repo.OutData[OwnAttackerId].Y = Target.Y;
repo.OutData[OwnAttackerId].W = Target.W;
//DataHandler.WriteReference(Target, Attacker_Blue);
}
}
public object Clone()
{
RobotParameters roboparams = new RobotParameters();
roboparams.Id = _id;
roboparams.ChipSpeed = _chipSpeed;
roboparams.TargetAddress = _targetAddress;
roboparams.Grab = _grab;
roboparams.IsBlue = _isBlue;
roboparams.KickSpeed = _kickSpeed;
roboparams.TimeStamp = _timeStamp;
roboparams.W = w;
roboparams.WVelocity = _wVel;
roboparams.Wheel = _wheel != null ? (float[])_wheel.Clone() : null;
roboparams.WheelAngles = _wheelAngles != null ? (float[]) _wheelAngles.Clone() : null;
roboparams.WheelSpeed = _wheelSpeed;
roboparams.X = x;
roboparams.XVelocity = _xVel;
roboparams.Y = y;
roboparams.YVelocity = _yVel;
roboparams.Spin = Spin;
return roboparams;
}
public void Reset()
{
errSumx = 0.0f;
lastUpdatex = DateTime.Now;
errSumy = 0.0f;
lastUpdatey = DateTime.Now;
errSumw = 0.0f;
lastUpdatew = DateTime.Now;
RobotParameters roboparams = new RobotParameters();
roboparams.Id = _id;
roboparams.XVelocity = 0;
roboparams.YVelocity = 0;
roboparams.WVelocity = 0;
}
public void GoalieCalculation(SSL_DetectionBall ball, int id)
{
IRobotInfo final = new RobotParameters(0, 0, 0);
final.Y = 350 * (ball.y / 2025);
final.X = -2975 + (500 * ((ball.x + 3025) / (6050)));
final.W = (float)Math.Atan2((ball.y - final.Y), (ball.x - final.X));
repo.OutData[id].X = final.X;
repo.OutData[id].Y = final.Y;
repo.OutData[id].W = final.W;
//DataHandler.WriteReference(final, id);
}
/// <summary>
/// Converts SSL detection Robot to Robot Info. Normally to use the location of robot in functions.
/// </summary>
/// <param name="robot"> SSL Detection robot to be converted.</param>
/// <returns> returns the converted robot of type RobotInfo.</returns>
public IRobotInfo RobotToIRobotInfo(SSL_DetectionRobot robot)
{
IRobotInfo newRobot = new RobotParameters();
newRobot.X = robot.x;
newRobot.Y = robot.y;
newRobot.W = robot.orientation;
return newRobot;
//return new IRobotInfo(robot.x, robot.y, robot.orientation);
}
//Now that we have some variables/references, we can start by declaring each one of them. So we do it here in the constructor
public QuickModuleTutorial()
{
//Lets start with the receiver
receiver = new SSLVisionReceiver(IPAddress.Parse("224.5.23.2"), 10002);
//The constructor to receiver needs the multicast IP and port on which the SSL Vision server is multicasting its packets,
//These can be found on SSL-Vision settings. Do make sure that your SSL-vision and network is correctly set up.
//You can test this by first connecting the receiver
receiver.Connect();
//Then making a call on receive
object packet = receiver.Receive();
if(packet==null)
throw new Exception("You messed up. Check your network and SSL Vision settings. ");
//Next, we have the sender
//Now we provide two kinds of senders, one for GRSim simulator, and another made for XBEE modules, initialize the one you need
sender = new GRSimSender(IPAddress.Loopback, 10020);
//Again, the IP address and port are the ones set up on GRSim. Verify them and that your network is good.
//You can test the sender too, first connect it
sender.Connect();
//Now create a RobotParameters object and set some random values i.e.
RobotParameters roboparams = new RobotParameters();
roboparams.IsBlue = true;
roboparams.Id = 1;
roboparams.XVelocity = 2;
//And send this packet through the sender
sender.Send(roboparams);
//If the robot specified in the robotId above doesn't show any movement, you messed up again.
//If you need the XBEE sender, then initialize it as
//sender = new XBeeSender("COM1");
//where COM1 is the port you connected the XBEE module to
//Ofcourse the XBEE sender needs the individual velocities to send them, so you need to set the angles between the wheels in each IRobotInfo's
//WheelAngles property before sending the packet
//Now comes the controllers, the one we provide is a PIDController, which is a modified implementation of PID by this guy Ron Beyer
//Firstly, you need to initialize the array of the controllers with length equal to the number of robots you have, or you like to move
//Lets say you are controlling six robots
controllers = new PIDController[6];
//next, initialize each controller with parameters of the robot
for (int i = 0; i < controllers.Length; i++)
{
controllers[i] = new PIDController(i, 2, 0, 0, 3025, -3025, 2, -2, 2, 0, 0, 2025, -2025, 2, -2, 5, 0, 0,
2*Math.PI, -2*Math.PI, Math.PI, -Math.PI);
//Yes its a hell lot of arguments and we did give out a much smaller constructor with default values, but thats the problem, they're default and may not suit
//your environment. Try to use this constructor. As for what the parameters are, I am not going to spill it out here, I've already given it in the constructor
//summary, go check there
}
//Now comes the module, which is completely your responsibility. Its the module that caused us to get into all this shit. Its the module that caused us to
//remodel our original program to have only one button named "Deep Shit"
//Yup you guessed it, the planner.
//We're not giving out the complete intelligence planner yet even though it contains some basic calculation functions,
//we do provide a manual controller, which can be used with a Joystick, so connect a joystick
//but for now, lets stick to some basics behavior, i.e. a test behavior defined in a dummy planner is to follow all the opponent robots
//Initialize the dummy planner as follows
planner = new DummyPlanner();
//Now if you have a working SSL-Vision receiver, you can also safely declare a referee receiver, its not currently being used but you'll need it
//when you write your own logic for obeying referee commands
//refereeReceiver = new SSLRefereeReceiver(IPAddress.Parse("224.5.23.3"), 10003);
//At this point, you have the absolute minimum things you need to run your system.
//Now you can make individual calls to each of the modules again and agin like
SSL_WrapperPacket receivedPacket = (SSL_WrapperPacket) receiver.Receive();
if (receivedPacket != null)
{
IRobotInfo[] plannedPoints = planner.PlanExclusive(receivedPacket);
if (plannedPoints != null)
{
for (int i = 0; i < plannedPoints.Length; i++)
{
plannedPoints[i] = controllers[i].ComputeExclusive(plannedPoints[i],
receivedPacket.detection.robots_blue[plannedPoints[i].Id]);
}
foreach(var pointWithVelocity in plannedPoints)
sender.Send(pointWithVelocity);
}
}
//and putting it in an infinite loop, which is pretty hideous if you ask me
// or you can automate the process with the rest of useful architecture we provided, for example;
//The data repository is a kind of utility we created to parse the incomming commands and store incomming/outgoing data in an organized way,
//its a good software practice so if you want to use it, you can initialize it too
dataRepository = new DataRepository(3, true);
//The repository also stores previous packets, which is useful for interpolation techniques. The size of history is given as the constructor argument
//The repository can be plugged in each of the above modules as follows. The modules will automatically read/write on repository
IDataSource sourceReference = (IDataSource) receiver;
sourceReference.Repository = dataRepository;
sourceReference = (IDataSource) sender;
sourceReference.Repository = dataRepository;
foreach (var controller in controllers)
{
sourceReference = (IDataSource) controller;
sourceReference.Repository = dataRepository;
}
sourceReference = (IDataSource) planner;
sourceReference.Repository = dataRepository;
//To keep track of your game configuration, configure the data source too, i.e. add the id's of the robots that you are using. you can access it in your planner
for (int i = 0; i < 6; i++)
dataRepository.Configuration.AddRobot(i, null);
//Its also advised to use the OnRefereeCommandChanged method to notify the planner whenever a change in referee command status occurs
//This is done by assigning the method as follows
dataRepository.OnRefereeCommandChanged += planner.OnRefereeCommandChanged;
//And we provide execution engines, which, as the name says execute your tasks. All you have to do is define a sequence of tasks
//First select an anchor task which acts as starting point and branches onto all other tasks.
//Now each of the above modules also implements and ITask interface which is essentially what each ExecutionEngine uses
//So, evidently, the sequence begins with reception of packet. So here's the anchor
ITask anchor = (ITask) receiver;
//To build the sequence of what comes next, we assign a delegate to the GetNext property of the anchor. The delegate returns an array of tasks,
//These tasks are the ones the engine will perform after the anchor's task has been performed.
//The next task should be to plan, so
anchor.GetNext = () => new[] {(ITask) planner};
//I've used the fancy lambda expression. It may look berserk but once you get to understand them they're quite handy
//Alternately, you can create a method with a signature matching the GetNextTasks, and in its body, return an array of ITask, i.e. the tasks you want performed next
//This is how:
//
//public ITask[] plannerReturner()
//{
// ITask[] array = new ITask[1];
// array[0] = (ITask) planner;
// return array;
//}
//
//And assign it
//anchor.GetNext = new GetNextTasks(plannerReturner);
//Next we take the planner and assign it a next sequence of tasks, which would be the controllers
ITask anotherReference = (ITask) planner;
anotherReference.GetNext = () =>
{
ITask[] controllerTasks = new ITask[controllers.Length];
for (int i = 0; i < controllers.Length; i++)
controllerTasks[i] = (ITask) controllers[i];
return controllerTasks;
};
//Next the controllers need to queue the tasks to send the data
foreach (var controller in controllers)
{
anotherReference = (ITask) controller;
anotherReference.GetNext = () => new[] {(ITask) sender};
}
//and the sender is the terminal
//Now lets bring in the execution engine, i'm going to go with the single processor, parallel processor's kinda experimental. Feel free to play with it if you want
executionEngine = new SingleProcessor(new[] {anchor});
//The processor takes an array of starting points/anchors.
//Now all you have to di is start the engine
executionEngine.Start();
//And voila, you're done. Each module's ITask interface exposes the Execute() method which is called by the processor.
//Remember how we plugged in a repository to each module, each module's Execute() takes care of taking data and writing it.
//Ofcourse, to keep this model working, I would advise you to follow the design pattern here. i.e. If you create a module
//by yourself according to your needs, it needs to implement one of the module specific interfaces e.g. IPacketSender for
//anything that sends outgoing packets, IPlanner for strategy planning/path planning modules, IController for any other
//position to velocity generation controller. The module must also implement two additional interfaces i.e. ITask and IDataSource
//after which the module can easily be plugged in this model.
//This project is a work in progress, and also contains a Rig module that has all of the above done in it, and a GUI that
//can completely configure and manage multiple Rigs. We hope to complete it soon. This tutorial is an absolute basic,
//you can navigate to different classes for better understanding. Feel free to ask us if you don't understand anything about
//this project, YES ONLY THIS PROJECT, don't bother us if you don't know how to work around events, or if you don't understand
//the concept of Threads, References or environment specific stuff. Try searching for these before.
//Finally, we're all humans and we all make mistakes. This thing is not error proof, heck its not even tested properly.
//So if you do find any bugs do feel free to report them to us so we can reflect it here. Remember, the basic purpose is
//to learn, so don't be selfish. We're kind enough to give our work out to you, be kind yourselves.
//Also take into consideration to contribute to this project and become an active member of something that might help a lot
//of generations to come.
}
/// <summary>
/// Initialize the structure to store 'size' robot's outgoing data. Its advised to initialize
/// </summary>
/// <param name="size">Total number of robots</param>
public void Initialize(int size)
{
_size = size;
_robotParams = new IRobotInfo[size];
_clearedRobots = new HashSet<int>();
for (int i = 0; i < size; i++)
{
_robotParams[i] = new RobotParameters();
_robotParams[i].IsBlue = _parent.Configuration.IsBlueTeam;
float[] angles;
if (_parent.Configuration.IsRobotInUse(i) && (angles = _parent.Configuration.GetWheelAngels(i)) != null)
{
_robotParams[i].WheelAngles = angles;
}
}
_robotParamLock = new object[size];
_robotParamLock = new object();
}