public ModuleState Start()
{
// clear all old commands
commandsQueue.Clear();
commandsQueue.InitPosition(new double[2] {
-1000, 0
});
if (bounderyConditions == null)
{
bounderyConditions = new PointParams();
}
else
{
bounderyConditions.Clear();
}
if (internalState == ModuleState.Inactive)
{
Guidance = new Thread(new ThreadStart(motionGuidanceStrategy.MotionGuidance));
internalState = ModuleState.Active;
motionGuidanceStrategy.Activate();
Guidance.Start();
}
return(internalState);
}
public override PointParams ActionPlanning(AHEntities.ActionDirective action, bool isNewPlaning)
{
AHEntities.Action A = action.Action;
PointParams finalConditions = null;
Random random = new Random();
Point puckP, puckV;
physicalState = worldModel.GetPhysicalState();
puckP = new Point(physicalState["PuckX"], physicalState["PuckY"]);
puckV = new Point(physicalState["PuckVx"], physicalState["PuckVy"]);
global = worldModel.GetSize();
Point crossing;
double[] crossParams;
switch (A)
{
case AHEntities.Action.BLOCK:
#region BLOCK
crossParams = AHEntities.EstimateLineCrossing.Estimate(puckP, puckV, rp,
AHEntities.EstimateLineCrossing.blockLine, global[0], global[1]);
crossing = new Point(AHEntities.EstimateLineCrossing.blockLine, crossParams[0]);
Point defendPoint = AHEntities.EstimateLineCrossing.CalculateActionPoint(crossing,
new Point(crossParams[2], crossParams[3]), rp, rm);
mLogger.AddLogMessage("LowLevel Planning BLOCK: estimated collission: mallet: " + defendPoint.ToString() +
", puck: " + crossing.ToString());
finalConditions = new PointParams();
finalConditions.AddParameters(defendPoint);
finalConditions.AddParameters(new Point(0, 0));
finalConditions.T = DateTime.Now + TimeSpan.FromSeconds(crossParams[1]);
break;
#endregion BLOCK
case AHEntities.Action.LEAVE:
#region LEAVE
finalConditions = null;
break;
#endregion LEAVE
case AHEntities.Action.ATTACK_RIGHT:
#region ATTACK_RIGHT
//finalConditions = CalculateDirectedAttack(AHEntities.Action.ATTACK_RIGHT, puckP, puckV, physicalState["PuckR"],
// AHEntities.EstimateLineCrossing.attackLine, global[0], global[1]);
//break;
#endregion ATTACK_RIGHT
case AHEntities.Action.ATTACK_LEFT:
#region ATTACK_LEFT
//finalConditions = CalculateDirectedAttack(AHEntities.Action.ATTACK_LEFT, puckP, puckV, physicalState["PuckR"],
// AHEntities.EstimateLineCrossing.attackLine, global[0], global[1]);
//break;
#endregion ATTACK_LEFT
case AHEntities.Action.ATTACK_MIDDLE:
#region ATTACK_MIDDLE
crossParams = AHEntities.EstimateLineCrossing.Estimate(puckP, puckV, rp,
AHEntities.EstimateLineCrossing.attackLine, global[0], global[1]);
mLogger.AddLogMessage("LowLevel: Puck Estimated at: (" + AHEntities.EstimateLineCrossing.attackLine.ToString() + "," + crossParams[0].ToString() + ") in " + crossParams[1].ToString() + " seconds");
Point puckPline = new Point(AHEntities.EstimateLineCrossing.attackLine, crossParams[0]);
double velocity = 1000;
finalConditions = CalculateNaiveDirectedAttack(A, puckPline, crossParams[1],
AHEntities.EstimateLineCrossing.attackLine, rp, rm, global[0], global[1], velocity);
break;
#endregion ATTACK_MIDDLE
case AHEntities.Action.DEFENSE_ATTACK:
#region DEFENSE_ATTACK
if (Math.Abs(puckV.X) < 0.01)
{
break;
}
crossParams = AHEntities.EstimateLineCrossing.Estimate(puckP, puckV, rp,
AHEntities.EstimateLineCrossing.defenseAttackLine, global[0], global[1]);
if (crossParams == null)
{
finalConditions = null;
break;
}
crossing = new Point(AHEntities.EstimateLineCrossing.defenseAttackLine, crossParams[0]);
Point attackPoint = AHEntities.EstimateLineCrossing.CalculateActionPoint(new Point(AHEntities.EstimateLineCrossing.defenseAttackLine, crossParams[0]),
new Point(crossParams[2], crossParams[3]), rp, rm);
mLogger.AddLogMessage("LowLevel Planning DEFENSE_ATTACK: estimated collission: mallet: " + attackPoint.ToString() +
", puck estimated at: " + crossing.ToString());
finalConditions = new PointParams();
finalConditions.AddParameters(attackPoint);
double yvel = (attackPoint.Y > 0) ? (yvel = -200) : (yvel = 200);
finalConditions.AddParameters(new Point(500, yvel));
finalConditions.T = DateTime.Now + TimeSpan.FromSeconds(crossParams[1]);
break;
#endregion DEFENSE_ATTACK
case AHEntities.Action.PREPARE:
#region PREPARE
finalConditions = new PointParams();
finalConditions.AddParameters(new Point(-1000, 0));
finalConditions.AddParameters(new Point(0, 0));
finalConditions.T = action.TimeStamp + TimeSpan.FromSeconds(0.5);
break;
#endregion PREPARE
case AHEntities.Action.STUCK_ATTACK:
#region STUCK_ATTACK
double stuckTime;
stuckTime = (action.Duration - (DateTime.Now - action.TimeStamp)).TotalSeconds;
Point stuckAttackPoint = AHEntities.EstimateLineCrossing.EstimateStuck(puckP, puckV, rp, global[0], global[1], stuckTime, rm);
if (stuckAttackPoint == null)
{
finalConditions = null;
break;
}
mLogger.AddLogMessage("LowLevel Planning STUCK_ATTACK: estimated collission: mallet: " + stuckAttackPoint.ToString());
finalConditions = new PointParams();
finalConditions.AddParameters(stuckAttackPoint);
finalConditions.AddParameters(new Point(0, 0));
finalConditions.T = action.TimeStamp + TimeSpan.FromSeconds(stuckTime);
break;
#endregion STUCK_ATTACK
default:
#region default
finalConditions = null;
break;
#endregion default
}
//saving current planning (action and parameters)
lastAction = A;
if (finalConditions != null)
{
if (lastPlan == null)
{
lastPlan = new PointParams();
}
lastPlan.Clear();
lastPlan.AddParameters(finalConditions.GetParamsByIndex(1));
lastPlan.AddParameters(finalConditions.GetParamsByIndex(2));
lastPlan.T = finalConditions.T;
}
else
{
lastPlan = null;
}
return(finalConditions);
}