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 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; }
}
// Apperently Not in use :/
// used for marking a robot, with given ID and a confidence to maintain distance, to stand inbetween target and defence and facing a particular point
public bool MarkOpponent(int ID, IRobotInfo source, IRobotInfo target, IRobotInfo defence, bool IsFacingTarget, float Confidence)
{
float r = Distance(defence.X, source.X, defence.Y, source.Y);
float theeta = (float)Math.Atan2(target.Y - defence.Y, target.X - defence.X);
float newx = defence.X + (r - Confidence) * (float)Math.Cos(theeta);
float newy = defence.Y + (r - Confidence) * (float)Math.Sin(theeta);
if (!IsFacingTarget)
{ theeta = -theeta; }
if (Distance(source.X, newx, source.Y, newy) > 100)
{
repo.OutData[ID].X = newx;
repo.OutData[ID].Y = newy;
repo.OutData[ID].W = theeta;
//DataHandler.WriteReference(new IRobotInfo(newx, newy, theeta), ID);
return false;
}
else
{ return true; }
}
public IRobotInfo PassCalculation(IRobotInfo target, IRobotInfo source, double Confidence)
{
double disX = source.X - target.X;
double disY = source.Y - target.Y;
double ShootDistance = Math.Sqrt(Math.Pow(disX, 2) + Math.Pow(disY, 2));
double ShootAngle = Math.Atan2(disY, disX);
ShootDistance += Confidence;
disX = ShootDistance * Math.Cos(ShootAngle) + target.X;
disY = ShootDistance * Math.Sin(ShootAngle) + target.Y;
return new RobotParameters((float)disX, (float)disY, (float)(Math.PI + Math.Atan2((disY - target.Y), (disX - target.X))));
//DataHandler.WriteReference(new IRobotInfo((float)disX,(float) disY, (float)(Math.PI + Math.Atan2((disY - target.y), (disX - target.x)))),(int)source.robot_id);
}
// Not being used right now :/
public IRobotInfo CalculateNearPoint(IRobotInfo SourcePoint, IRobotInfo ReferencePoint, float conf)
{
double disX = ReferencePoint.X - SourcePoint.X;
double disY = ReferencePoint.Y - SourcePoint.X;
double Distance = Math.Sqrt(Math.Pow(disX, 2) + Math.Pow(disY, 2));
double Angle = Math.Atan2(disY, disX);
Distance += conf;
return new RobotParameters((float)(Distance * Math.Cos(Angle) + SourcePoint.X), (float)(Distance * Math.Sin(Angle) + SourcePoint.Y), (float)Angle);
}
public IRobotInfo ComputeExclusive(IRobotInfo target, SSL_DetectionRobot current)
{
if (_repReference == null)
throw new NullReferenceException("The controller needs to read data through functions assigned to the delegates Read and Write. Assign them a method.");
IRobotInfo Reference = target;
SSL_DetectionRobot Feedback = current;
double pvx = (double)Math.Round((decimal)Feedback.x, 0, MidpointRounding.AwayFromZero);
double pvy = (double)Math.Round((decimal)Feedback.y, 0, MidpointRounding.AwayFromZero);
double pvw = (double)Math.Round((decimal)Feedback.orientation, 5, MidpointRounding.AwayFromZero);
float tempRobotAngle = Feedback.orientation;
double spx = (double)Math.Round((decimal)Reference.X, 0, MidpointRounding.AwayFromZero);
double spy = Reference.Y;
double spw = Reference.W;
#region 180 Degree Error Correction
if ((spw - pvw) < -Math.PI)
{ spw += (2 * Math.PI); }
else if ((spw - pvw) > Math.PI)
{ pvw += (2 * Math.PI); }
#endregion
pvx = Clamp(pvx, pvMinx, pvMaxx);
pvx = ScaleValue(pvx, pvMinx, pvMaxx, -1.0f, 1.0f);
pvy = Clamp(pvy, pvMiny, pvMaxy);
pvy = ScaleValue(pvy, pvMiny, pvMaxy, -1.0f, 1.0f);
pvw = Clamp(pvw, pvMinw, pvMaxw);
pvw = ScaleValue(pvw, pvMinw, pvMaxw, -1.0f, 1.0f);
spx = Clamp(spx, pvMinx, pvMaxx);
spx = ScaleValue(spx, pvMinx, pvMaxx, -1.0f, 1.0f);
spy = Clamp(spy, pvMiny, pvMaxy);
spy = ScaleValue(spy, pvMiny, pvMaxy, -1.0f, 1.0f);
spw = Clamp(spw, pvMinw, pvMaxw);
spw = ScaleValue(spw, pvMinw, pvMaxw, -1.0f, 1.0f);
double errx = spx - pvx;
double erry = spy - pvy;
double errw = spw - pvw;
double pTermx = errx * kpx;
double iTermx = 0.0f;
double dTermx = 0.0f;
double pTermy = erry * kpy;
double iTermy = 0.0f;
double dTermy = 0.0f;
double pTermw = errw * kpw;
double iTermw = 0.0f;
double dTermw = 0.0f;
double partialSumx = 0.0f;
DateTime nowTimex = DateTime.Now;
double partialSumy = 0.0f;
DateTime nowTimey = DateTime.Now;
double partialSumw = 0.0f;
DateTime nowTimew = DateTime.Now;
if (lastUpdatex != null)
{
double dTx = (nowTimex - lastUpdatex).TotalSeconds;
if (pvx >= pvMinx && pvx <= pvMaxx)
{
partialSumx = errSumx + dTx * errx;
iTermx = kix * partialSumx;
}
if (dTx != 0.0f)
dTermx = kdx * (pvx - lastPVx) / dTx;
}
if (lastUpdatey != null)
{
double dTy = (nowTimey - lastUpdatey).TotalSeconds;
if (pvy >= pvMiny && pvy <= pvMaxy)
{
partialSumy = errSumy + dTy * erry;
iTermy = kiy * partialSumy;
}
if (dTy != 0.0f)
dTermy = kdy * (pvy - lastPVy) / dTy;
}
if (lastUpdatew != null)
{
double dTw = (nowTimew - lastUpdatew).TotalSeconds;
if (pvw >= pvMinw && pvw <= pvMaxw)
{
partialSumw = errSumw + dTw * errw;
iTermw = kiw * partialSumw;
}
if (dTw != 0.0f)
dTermw = kdw * (pvw - lastPVw) / dTw;
}
lastUpdatex = nowTimex;
errSumx = partialSumx;
lastPVx = pvx;
lastUpdatey = nowTimey;
errSumy = partialSumy;
lastPVy = pvy;
lastUpdatew = nowTimew;
errSumw = partialSumw;
lastPVw = pvw;
double outRealx = pTermx + iTermx + dTermx;
double outRealy = pTermy + iTermy + dTermy;
double outRealw = pTermw + iTermw + dTermw;
#region Rotation Transform for angle Compensation
PointF temp = RotatePoint(new PointF((float)outRealx, (float)outRealy), new PointF(0, 0), -tempRobotAngle);
outRealx = temp.X;
outRealy = temp.Y;
#endregion
outRealx = Clamp(outRealx, -1.0f, 1.0f);
outRealx = ScaleValue(outRealx, -1.0f, 1.0f, outMinx, outMaxx);
outRealy = Clamp(outRealy, -1.0f, 1.0f);
outRealy = ScaleValue(outRealy, -1.0f, 1.0f, outMiny, outMaxy);
outRealw = Clamp(outRealw, -1.0f, 1.0f);
outRealw = ScaleValue(outRealw, -1.0f, 1.0f, outMinw, outMaxw);
//Write it out to the world
IRobotInfo roboparams = Reference;
roboparams.Id = _id;
roboparams.XVelocity = (float)outRealx;
roboparams.YVelocity = (float)outRealy;
roboparams.WVelocity = (float)outRealw;
return roboparams;
}
/// <summary>
/// Used to go to ball turn on the dribbler to grab the ball.
/// </summary>
/// <param name="ID"> Id of robot to move.</param>
/// <param name="source"> Location of Robot to be moved.</param>
/// <param name="ball"> Location of ball to grab.</param>
/// <returns>returns true when reached and grabbed ball else returns fale.</returns>
public bool GrabBall(int ID, IRobotInfo source, SSL_DetectionBall ball)
{
if (Distance(source.X, ball.x, source.Y, ball.y) > 105)
{
GotoPoint(ID, source, new RobotParameters(ball.x, ball.y, (float)Math.Atan2((ball.y - source.Y), (ball.x - source.X))), 0);
return false;
}
else
{
repo.OutData[ID].Grab = true;
//DataHandler.Dribble[ID] = true;
return true;
}
}
/// <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 GoNearandRotate(SSL_WrapperPacket packet, bool IsBlue, float conf, IRobotInfo target)
{
if (Distance(packet.detection.robots_blue[OwnAttackerId].x, packet.detection.balls[0].x, packet.detection.robots_blue[OwnAttackerId].y, packet.detection.balls[0].y) > conf)
GoNearBall(packet, true, conf - 200);
else
{
rotateabout(packet, target, 350);
}
}
/// <summary>
/// Used to move a robot to a point with specific angle (w).
/// </summary>
/// <param name="ID"> Id of robot to move.</param>
/// <param name="source"> The Position of Robot to move.</param>
/// <param name="Destination"> The point (x,y,w) of the destination.</param>
/// <param name="confidence"> The confidence value that is considered as close to destination.</param>
/// <returns> returns false if the robot is not withing the confidence of the destination otherwise true.</returns>
public bool GotoPoint(int ID, IRobotInfo source, IRobotInfo Destination, float confidence)
{
if (Distance(source.X, Destination.X, source.Y, Destination.Y) > confidence)
{
repo.OutData[ID].X = Destination.X;
repo.OutData[ID].Y = Destination.Y;
repo.OutData[ID].W = Destination.W;
//DataHandler.WriteReference(Destination, ID);
return false;
}
else
{ return true; }
}
private byte[] CreateDataBytes(IRobotInfo packet)
{
byte[] data = new byte[5];
packet.ToIndividualWheels();
byte[] PWMs = Quantize(packet);
for (int i = 0; i < PWMs.Length; i++)
{
data[i] = PWMs[i];
}
byte lastByte = 0x00;
if (packet.KickSpeed > 0 && packet.KickSpeed <= 10)
{
lastByte = (byte)packet.KickSpeed;
}
//SET DRIBBLER
lastByte = Statics.SetBit(lastByte, 7, packet.Grab);
//SET FLAT KICK
if (packet.KickSpeed > 0)
{ lastByte = Statics.SetBit(lastByte, 6, true); }
if (packet.ChipSpeed > 0)
{ lastByte = Statics.SetBit(lastByte, 5, true); }
data[4] = lastByte;
return data;
}
protected byte[] Quantize(IRobotInfo packet)
{
float multiplier = (127f / 2f);
byte[] PWMs = new byte[4];
for (int i = 0; i < PWMs.Length; i++)
{
byte temp = 0x00;
if (packet.Wheel[i] < 0) //if negative
{
float tempvel = Math.Abs(packet.Wheel[i]);
temp = (byte)(tempvel * multiplier);
temp = Statics.SetBit(temp, 7, true);
}
else //if zero or positive
{
temp = (byte)(packet.Wheel[i] * multiplier);
}
PWMs[i] = temp;
}
return PWMs;
}
public void Send(IRobotInfo Packet)
{
byte[] data = CreateDataBytes(Packet);
Int64 DestinationAddress = Convert.ToInt64(Packet.TargetAddress, 16);
byte[] XbeePacket = _packetMaker.MakePacket(DestinationAddress, data);
string show = ByteArrayToString(XbeePacket);
_serialPort.Write(XbeePacket, 0, XbeePacket.Length);
}
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;
}
/// <summary>
/// Checks for a point weather it lies inside a polygon or not.
/// Normally used to find out if line of sight is clear for a pass or a goal.
/// </summary>
/// <param name="polygon"> Array of points that make a polygon. Must be atleast 3 points.</param>
/// <param name="point"> The point to check if it lies inside the polygon.</param>
/// <returns>returns true if point lies inside the polygon and false if it lies outside.</returns>
public bool IsPointInPolygon(PointF[] polygon, IRobotInfo point)
{
bool isInside = false;
for (int i = 0, j = polygon.Length - 1; i < polygon.Length; j = i++)
{
if (((polygon[i].Y > point.Y) != (polygon[j].Y > point.Y)) &&
(point.X < (polygon[j].X - polygon[i].X) * (point.Y - polygon[i].Y) / (polygon[j].Y - polygon[i].Y) + polygon[i].X))
{
isInside = !isInside; //Not sure if this is correct shouldnt it be just true? or a break statment after it
}
}
return isInside;
}
public bool TriangleTest(IRobotInfo p1, IRobotInfo p2, IRobotInfo p3, IRobotInfo p)
{
float alpha = ((p2.Y - p3.Y) * (p.X - p3.X) + (p3.X - p2.X) * (p.Y - p3.Y)) /
((p2.Y - p3.Y) * (p1.X - p3.X) + (p3.X - p2.X) * (p1.Y - p3.Y));
float beta = ((p3.Y - p1.Y) * (p.X - p3.X) + (p1.X - p3.X) * (p.Y - p3.Y)) /
((p2.Y - p3.Y) * (p1.X - p3.X) + (p3.X - p2.X) * (p1.Y - p3.Y));
float gamma = 1.0f - alpha - beta;
if (alpha > 0 && beta > 0 && gamma > 0)
return true;
else
return false;
}
/// <summary>
/// Used to check if the ball is withing kicking radius and kicks the ball.
/// </summary>
/// <param name="ID"> Id of robot to kick the ball.</param>
/// <param name="source"> Location of robot that will kick the ball.</param>
/// <param name="ball"> Location of ball.</param>
/// <param name="KickSpeed"> Speed by which ball will be kicked.</param>
/// <returns> returns true of ball is in radius of kicking and is kicked otherwise false.</returns>
public bool KickBall(int ID, IRobotInfo source, SSL_DetectionBall ball, float KickSpeed)
{
if (Distance(source.X, ball.x, source.Y, ball.y) < 105)
{
repo.OutData[ID].KickSpeed = KickSpeed;
//DataHandler.WriteKickSpeed(KickSpeed, ID);
return true;
}
else
{ return false; }
}
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>
/// 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;
}
/// <summary>
/// Used to check line of sight between source point and target point for the checkpoints.
/// </summary>
/// <param name="Source"> The Starting point. Normally Position of a robot to give pass.</param>
/// <param name="Target"> The Target point. Normally position of a robot to receive pass.</param>
/// <param name="checkpoints"> Points to be checked . Normally position of all other robots.</param>
/// <returns> returns true if line of Sight is NOT clear and false if it is Clear.</returns> //Should be opposite :/
public bool CheckLineOfSight(IRobotInfo Source, IRobotInfo Target, IRobotInfo[] checkpoints)
{
bool IsBlocked = false;
float radius = 200;
PointF[] points = new PointF[4];
float angle = (float)Math.Atan2(Target.Y - Source.Y, Target.X - Source.X);
points[0] = new PointF((float)(-radius * Math.Sin(angle)) + Source.X, (float)(radius * Math.Cos(angle)) + Source.Y);
points[1] = new PointF((float)(radius * Math.Sin(angle)) + Source.X, (float)(-radius * Math.Cos(angle)) + Source.Y);
points[3] = new PointF((float)(-radius * Math.Sin(angle)) + Target.X, (float)(radius * Math.Cos(angle)) + Target.Y);
points[2] = new PointF((float)(radius * Math.Sin(angle)) + Target.X, (float)(-radius * Math.Cos(angle)) + Target.Y);
foreach (IRobotInfo checkpoint in checkpoints)
{
IsBlocked = IsPointInPolygon(points, checkpoint);
if (IsBlocked)
{
return true;
}
}
return false;
}
public RobotBehaviour(IRobotInfo robotInfo, IPlateauInfo plateauInfo)
{
_robotInfo = robotInfo;
_plateauInfo = plateauInfo;
}
