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 SimpleLinesActionPlanningStrategy(WorldModel WM)
{
worldModel = WM;
mLogger = Logger.Logger.Instance;
random = new Random();
lastPlan = null;
lastAction = AHEntities.Action.LEAVE;
}
private PointParams CalculateDirectedAttack(AHEntities.Action direction, Point puckP, Point puckV, double puckR,
double xline, int tableW, int tableH)
{
PointParams finalConditions = new PointParams();
double[] crossParamsAttack = AHEntities.EstimateLineCrossing.Estimate(puckP, puckV, puckR, xline, tableW, tableH);
Point XYi = GetCollissionPoint(puckV.X, puckV.Y, xline, crossParamsAttack[0], direction);
// Detemine speeds at the goal.
Point Pg = new Point(1180, 0);
Point speedG = GetGoalSpeed(direction, xline, crossParamsAttack[0], Pg);
double thetaG = -speedG.X / rp; // constrain on the angle speed at the goal
double vG = speedG.Norm();
double vPS;
Point speedPS;
double thetaPS, thetaPSdot;
if (direction != AHEntities.Action.ATTACK_MIDDLE)
{
// calculating angles
double alpha = CalculateAlpha(speedG, AHEntities.Action.ATTACK_RIGHT);
double beta = CalculateBeta(direction, xline, crossParamsAttack[0], tableH, Pg, alpha);
// calculating post strike prameters
vPS = -vG / e * Math.Sin(alpha) / Math.Sin(beta);
speedPS = new Point(vPS * Math.Cos(beta), vPS * Math.Sin(beta));
thetaPS = Math.Atan2(speedPS.Y, speedPS.X);
thetaPSdot = 2 * vG / (rp * e) * Math.Sin(alpha) / Math.Tan(beta) - 3 * vG / rp * Math.Cos(alpha);
}
else // direction == MIDDLE
{
// calculating post strike prameters
vPS = 0;
speedPS = new Point();
thetaPS = 0;
thetaPSdot = 0;
}
// calculating frames rotation angle
double thetaP = Math.Atan2(puckV.Y, puckV.X);
double thetaC = CalculateRotationAngle(thetaPS, thetaPSdot, thetaP, puckR, vPS, puckV.Norm());
// transform puck and ps velocities to impact frame
Point impactVps = TransformFrames(thetaC, speedPS, true);
Point impactVp = TransformFrames(thetaC, puckV, true);
// calculate control velocities
Point impactVm = CalculateImpactVelocities(impactVp, impactVps, thetaP, thetaPS, rp);
// transform to XY
Point vM = TransformFrames(thetaC, impactVm, false);
// create finalConditions.
Point target = CalculateTargetPoint(XYi, new Point(AHEntities.EstimateLineCrossing.attackLine, crossParamsAttack[0]));
finalConditions.AddParameters(target);
finalConditions.AddParameters(vM);
finalConditions.T = DateTime.Now + TimeSpan.FromSeconds(crossParamsAttack[1]);
return(finalConditions);
}
public bool PointAvailable(int index)
{
int ind = index;
if (((DivePoints[ind].Depth < DivePoints[ind - 1].Depth) && ((DivePoints[ind].Depth - DivePoints[ind - 1].Depth) / (DivePoints[ind - 1].Time - DivePoints[ind].Time) > 18)) || (DivePoints[ind - 1].Time >= DivePoints[ind].Time))
{
return(false);
}
else
{
List <DivePoint> myDivePoint = new List <DivePoint>();
CompartmentParams compartmentParams = new CompartmentParams(DivePoints[0].Gas.Helium);
double FGas = 1 - DivePoints[0].Gas.Oxygen;
PointParams truePointParams = new PointParams();
truePointParams.pCurrent = new double[17];
for (int i = 0; i < truePointParams.pCurrent.Length; i++)
{
truePointParams.pCurrent[i] = pressureAmbient(FGas, 0, pressureSeaLevel);
}
int count = index;
int j = 0;
double airConsumption = 0;
double vConsumption = 30;
double[] check = new double[17];
double max = 0;
while (count > 0)
{
compartmentParams = new CompartmentParams(DivePoints[j].Gas.Helium);
FGas = 1 - DivePoints[j].Gas.Oxygen;
double vRate = (DivePoints[j + 1].Depth - DivePoints[j].Depth) / (DivePoints[j + 1].Time - DivePoints[j].Time);
airConsumption += findAirConsumption(vConsumption, DivePoints[j + 1].Time - DivePoints[j].Time, DivePoints[j].Depth, DivePoints[j + 1].Depth);
for (int i = 0; i < truePointParams.pCurrent.Length; i++)
{
truePointParams.pCurrent[i] = pressureAtoB(pressureAmbient(FGas, DivePoints[j].Depth, pressureSeaLevel), FGas, vRate, DivePoints[j + 1].Time - DivePoints[j].Time, Kf(compartmentParams.compartment.paramT[i]), truePointParams.pCurrent[i]);
}
j += 1;
count -= 1;
}
for (int i = 0; i < truePointParams.pCurrent.Length; i++)
{
check[i] = truePointParams.pCurrent[i] - pressureInCompartment(compartmentParams.compartment.paramM0[i], compartmentParams.compartment.paramdM[i], DivePoints[ind].Depth);
if (check[i] > max)
{
max = check[i];
}
}
if (max > 0)
{
return(false);
}
else
{
return(true);
}
}
}
public void GetAscentByIndex(int index)
{
List <DivePoint> myDivePoint = new List <DivePoint>();
CompartmentParams compartmentParams = new CompartmentParams(DivePoints[0].Gas.Helium);
double FGas = 1 - DivePoints[0].Gas.Oxygen;
PointParams truePointParams = new PointParams();
PointParamsAscent truePointParamsAscent = new PointParamsAscent();
truePointParams.pCurrent = new double[17];
for (int i = 0; i < truePointParams.pCurrent.Length; i++)
{
truePointParams.pCurrent[i] = pressureAmbient(FGas, 0, pressureSeaLevel);
}
int count = index;
int j = 0;
double airConsumption = 0;
double vConsumption = 30;
while (count > 0)
{
compartmentParams = new CompartmentParams(DivePoints[j].Gas.Helium);
FGas = 1 - DivePoints[j].Gas.Oxygen;
double vRate = (DivePoints[j + 1].Depth - DivePoints[j].Depth) / (DivePoints[j + 1].Time - DivePoints[j].Time);
airConsumption += findAirConsumption(vConsumption, DivePoints[j + 1].Time - DivePoints[j].Time, DivePoints[j].Depth, DivePoints[j + 1].Depth);
for (int i = 0; i < truePointParams.pCurrent.Length; i++)
{
truePointParams.pCurrent[i] = pressureAtoB(pressureAmbient(FGas, DivePoints[j].Depth, pressureSeaLevel), FGas, vRate, DivePoints[j + 1].Time - DivePoints[j].Time, Kf(compartmentParams.compartment.paramT[i]), truePointParams.pCurrent[i]);
}
j += 1;
count -= 1;
}
compartmentParams = new CompartmentParams(DivePoints[j].Gas.Helium);
FGas = 1 - DivePoints[j].Gas.Oxygen;
double depthTo3 = maxDepthTo3(truePointParams.pCurrent, compartmentParams.compartment.paramM0, compartmentParams.compartment.paramdM, pressureSeaLevel);
truePointParams.airConsumption = airConsumption;
truePointParams.Gas = DivePoints[j].Gas;
double lastDepth = DivePoints[j].Depth;
double lastTime = DivePoints[j].Time;
truePointParamsAscent = ascentUp(truePointParams, pressureSeaLevel, depthTo3, lastTime, lastDepth);
for (int i = 0; i < truePointParamsAscent.depth.Count; i++)
{
DivePoint dv = new DivePoint(truePointParamsAscent.time[i], truePointParamsAscent.depth[i]);
myDivePoint.Add(dv);
}
DivePoints.RemoveRange(index + 1, DivePoints.Count - 1 - index);
DivePoints.AddRange(myDivePoint);
}
private PointParams CalculateNaiveDirectedAttack(AHEntities.Action direction, Point puckPline, double time,
double xline, double puckRadius, double malletRadius, int tableW, int tableH, double velocity)
{
PointParams finalConditions = new PointParams();
Point collisionPoint, targetPoint, targetVelocity;
collisionPoint = GetNaiveCollisionPoint(direction, puckPline, puckRadius - 2, tableW, tableH);
targetPoint = GetNaiveTargetPoint(puckPline, collisionPoint, malletRadius - 2);
targetVelocity = GetNaiveTargetVelocity(collisionPoint, targetPoint, velocity);
finalConditions.AddParameters(targetPoint);
finalConditions.AddParameters(targetVelocity);
finalConditions.T = DateTime.Now + TimeSpan.FromSeconds(time);
return(finalConditions);
}
public void PlanNewAction(ActionDirective A, bool isNewPlanning)
{
planTime.Restart();
// target point parameters (for the whole motion)
//if (isNewPlanning)
//bounderyConditions = actionPlanningStrategy.ActionPlanning(A, isNewPlanning);
bounderyConditions = actionPlanningStrategy.ActionPlanning(A, true);
if (bounderyConditions == null)
{
return;
}
state = WM.GetPhysicalState();
Point agentP = new Point(state["AgentX"], state["AgentY"]);
Point agentV = new Point(state["AgentVx"], state["AgentVy"]);
mLogger.AddLogMessage("LowLevel: action planned: agent currently at: " + agentP.ToString() + " and: " + agentV.ToString() + ", target: " + bounderyConditions.ToString());
// initial movement parameters
PointParams initialConditions = new PointParams();
initialConditions.AddParameters(agentP);
initialConditions.AddParameters(agentV);
initialConditions.T = DateTime.Now;
// new trajectory generation
double time = (bounderyConditions.T - initialConditions.T).TotalSeconds;
double[][,] newTrajectory = null;
if ((isNewPlanning) || ((time < maxTime) && (time > 0)))
{
newTrajectory = trajectoryPlanningStrategy.TrajectoryPlanning(initialConditions, bounderyConditions);
}
if (newTrajectory != null)
{
mLogger.AddLogMessage("LowLevel: New Trajectory Designed, Length: " + newTrajectory[0].LongLength.ToString() + " time: " + time.ToString());
commandsQueue.Replace(QueueType.Position, newTrajectory[0]);
// output the whole trajectory to log
//mLogger.AddLogMessage("LowLevel: Trajectory: " + commandsQueue.PositionToString());
commandsQueue.Replace(QueueType.Velocity, newTrajectory[1]);
}
planTime.Stop();
mLogger.AddLogMessage("LowLevel: Planning time was: " + planTime.Elapsed.TotalSeconds.ToString() + " Seconds");
}
public LowLevelLayer(Communicator.Communicator com, WorldModel worldModel, double timeStep, short degrees)
{
internalState = ModuleState.Inactive;
communicator = com;
WM = worldModel;
DOF = degrees;
Ts = timeStep;
mLogger = Logger.Logger.Instance;
planTime = new Stopwatch();
planTime.Reset();
Hashtable consts = WM.GetConstants();
maxTime = (double)consts["MoveInterval"];
commandsQueue = new TrajectoryQueue(DOF, new double[2] {
-1000, 0
});
bounderyConditions = new PointParams();
//motionGuidanceStrategy = new OpenLoopMotionGuidanceStrategy(commandsQueue, com, Ts);
motionGuidanceStrategy = new PDMotionGuidanceStrategy(commandsQueue, com, Ts, WM);
actionPlanningStrategy = new SimpleLinesActionPlanningStrategy(WM);
trajectoryPlanningStrategy = new PolynomialTrajectoryPlanningStrategy(Ts, maxTime);
// dummy planning to warm up the containers
#region Warmup
/*
* PointParams init = new PointParams();
* init.AddParameters(new Point(0, 0));
* init.AddParameters(new Point(0, 0));
* init.T = DateTime.Now;
* //init.T = 0;
* PointParams final = new PointParams();
* final.AddParameters(new Point(0, 0));
* final.AddParameters(new Point(0, 0));
* final.T = DateTime.Now + TimeSpan.FromSeconds(0.2);
* state = WM.GetPhysicalState();
* actionPlanningStrategy.ActionPlanning(AHEntities.Action.DEFENSE_ATTACK, true);
* trajectoryPlanningStrategy.TrajectoryPlanning(init, final);
*/
#endregion Warmup
}
public string EmergencyAscentMessage(int index)
{
string emergencyMessage = "";
Air air = new Air();
List <DivePoint> myDivePoint = new List <DivePoint>();
CompartmentParams compartmentParams = new CompartmentParams(DivePoints[0].Gas.Helium);
double FGas = 1 - DivePoints[0].Gas.Oxygen;
double[] check = new double[17];
double[] d = new double[17];
List <string> st = new List <string> {
};
double em = 0;
double maxD = 0;
double timeEm = 0;
PointParams truePointParams = new PointParams();
truePointParams.pCurrent = new double[17];
for (int i = 0; i < truePointParams.pCurrent.Length; i++)
{
truePointParams.pCurrent[i] = pressureAmbient(FGas, 0, pressureSeaLevel);
Console.WriteLine("первое давление = " + truePointParams.pCurrent[i]);
}
int count = index;
int j = 0;
double airConsumption = 0;
double vConsumption = 30;
while (count > 0)
{
compartmentParams = new CompartmentParams(DivePoints[j].Gas.Helium);
FGas = 1 - DivePoints[j].Gas.Oxygen;
double vRate = (DivePoints[j + 1].Depth - DivePoints[j].Depth) / (DivePoints[j + 1].Time - DivePoints[j].Time);
airConsumption += findAirConsumption(vConsumption, DivePoints[j + 1].Time - DivePoints[j].Time, DivePoints[j].Depth, DivePoints[j + 1].Depth);
for (int i = 0; i < truePointParams.pCurrent.Length; i++)
{
truePointParams.pCurrent[i] = pressureAtoB(pressureAmbient(FGas, DivePoints[j].Depth, pressureSeaLevel), FGas, vRate, DivePoints[j + 1].Time - DivePoints[j].Time, Kf(compartmentParams.compartment.paramT[i]), truePointParams.pCurrent[i]);
Console.WriteLine("давление после шага номер " + j + " = " + truePointParams.pCurrent[i]);
}
j += 1;
count -= 1;
}
compartmentParams = new CompartmentParams(DivePoints[j].Gas.Helium);
FGas = 1 - DivePoints[j].Gas.Oxygen;
for (int i = 0; i < truePointParams.pCurrent.Length; i++)
{
truePointParams.pCurrent[i] = pressureAtoB(pressureAmbient(FGas, DivePoints[j].Depth, pressureSeaLevel), FGas, -18, DivePoints[j].Depth / 18, Kf(compartmentParams.compartment.paramT[i]), truePointParams.pCurrent[i]);
check[i] = truePointParams.pCurrent[i] - pressureInCompartment(compartmentParams.compartment.paramM0[i], compartmentParams.compartment.paramdM[i], 0);
if (check[i] > em)
{
em = check[i];
}
}
compartmentParams = new CompartmentParams(air.Helium);
FGas = 1 - air.Oxygen;
if (em > 0)
{
for (int i = 0; i < truePointParams.pCurrent.Length; i++)
{
d[i] = (truePointParams.pCurrent[i] - compartmentParams.compartment.paramM0[i]) / compartmentParams.compartment.paramM0[i];
if (d[i] > maxD)
{
maxD = d[i];
}
}
string s = "Декомпрессионные параметры в барокамере:\n";
maxD = Math.Ceiling(maxD);
if (maxD % 3 != 0)
{
maxD = maxD - (maxD % 3) + 3;
}
while (maxD > 0)
{
timeEm = maxTimeToStop(truePointParams.pCurrent, compartmentParams.compartment.paramM0, compartmentParams.compartment.paramM0, pressureAmbient(FGas, maxD, 10), maxD, KF(compartmentParams.compartment.paramT));
for (int i = 0; i < truePointParams.pCurrent.Length; i++)
{
truePointParams.pCurrent[i] = pressurePoint(pressureAmbient(FGas, maxD, 10), truePointParams.pCurrent[i], Kf(compartmentParams.compartment.paramT[i]), timeEm);
}
string str = "";
str = $"поместить на {timeEm} минут при давлении {maxD / 10 + 1} Бар ";
st.Add(str);
maxD = maxD - 3;
}
emergencyMessage = s + string.Join("\n", st);
}
else
{
emergencyMessage = "Экстренное всплытие не требует декомпрессионных обязательств";
}
return(emergencyMessage);
}
private PointParamsAscent ascentUp(PointParams previousStep, double pressureSeaLevel, double depthTo3, double timeBeforeAscent, double lastDepth)
{
CompartmentParams compartmentParams = new CompartmentParams(previousStep.Gas.Helium);
PointParams pointParams;
pointParams.pCurrent = new double[17];
for (int i = 0; i < pointParams.pCurrent.Length; i++)
{
pointParams.pCurrent[i] = previousStep.pCurrent[i];
}
PointParamsAscent ascentParams;
ascentParams.depth = new List <double>();
ascentParams.time = new List <double>();
double timeBeforeStepAscent = timeBeforeAscent;
int vRate = -10;
int number = 1;
double vAscent = 18;
double vOnDepth = 9;
double airConsumption = (double)previousStep.airConsumption;
double timeToStop = 0;
double FGas = 1 - (double)previousStep.Gas.Oxygen;
double depthCurrent = lastDepth;
double depthNext = depthTo3;
ascentParams.airConsumption = airConsumption;
while (depthNext > 0)
{
double t = (depthNext - depthCurrent) / vRate;
for (int i = 0; i < pointParams.pCurrent.Length; i++)
{
pointParams.pCurrent[i] = pressureAtoB(pressureAmbient(FGas, depthCurrent, pressureSeaLevel), FGas, vRate, t, Kf(compartmentParams.compartment.paramT[i]), pointParams.pCurrent[i]);
}
airConsumption += findAirConsumption(vAscent, t, depthCurrent, depthNext);
timeToStop = maxTimeToStop(pointParams.pCurrent, compartmentParams.compartment.paramM0, compartmentParams.compartment.paramdM, pressureAmbient(FGas, depthNext, pressureSeaLevel), depthNext, KF(compartmentParams.compartment.paramT));
if (timeToStop > 0)
{
for (int i = 0; i < pointParams.pCurrent.Length; i++)
{
pointParams.pCurrent[i] = pressurePoint(pressureAmbient(FGas, depthNext, pressureSeaLevel), pointParams.pCurrent[i], Kf(compartmentParams.compartment.paramT[i]), timeToStop);
}
airConsumption += findAirConsumption(vOnDepth, timeToStop, depthNext, depthNext);
double d = depthNext;
ascentParams.depth.Add(d);
ascentParams.time.Add(timeBeforeStepAscent + t);
ascentParams.depth.Add(d);
ascentParams.time.Add(timeBeforeStepAscent + t + timeToStop);
timeBeforeStepAscent += t + timeToStop;
depthCurrent = depthNext;
depthNext -= 3;
number += 1;
}
else
{
double d = depthNext;
depthNext -= 3;
timeBeforeStepAscent += t;
}
}
ascentParams.depth.Add(0);
ascentParams.time.Add(timeBeforeStepAscent + 0.3);
ascentParams.airConsumption = airConsumption;
return(ascentParams);
}
public abstract double[][,] TrajectoryPlanning(PointParams initialConditions, PointParams finalConditions);
public override double[][,] TrajectoryPlanning(PointParams initialConditions, PointParams finalConditions)
{
if (finalConditions == null)
{
return(null);
}
double time = (finalConditions.T - initialConditions.T).TotalSeconds;
if ((double.IsInfinity(time)) || (time < 0))
{
return(null);
}
if (time < Ts)
{
time = Ts;
}
try
{
double[,] initialConditionsArray;
double[,] finalConditionsArray;
int counter = 0;
double finalTime, startTime = 0.0;
int size = (int)Math.Floor(time / Ts);
time = size * Ts;
finalTime = time;
double[][,] trajectory = new double[2][, ];
trajectory[0] = new double[2, size];
trajectory[1] = new double[2, size];
// check if motion is too long (longer than MaxMoveTime)
if (time > maxMoveTime)
{
startTime = time - maxMoveTime;
finalTime = maxMoveTime;
// zero velocity
initialConditions.EditParamByIndex(2, new Point(0, 0));
initialConditionsArray = initialConditions.GetParamsAsArray();
// motion is too long, setting an initial resting period
for (double t = Ts; t <= startTime + eps; t = t + Ts)
{
// Velocity
trajectory[1][0, counter] = 0;
trajectory[1][1, counter] = 0;
// path
trajectory[0][0, counter] = initialConditionsArray[0, 0];
trajectory[0][1, counter] = initialConditionsArray[1, 0];
t = Math.Round(t * 1000) / 1000;
counter++;
}
}
else
{
initialConditionsArray = initialConditions.GetParamsAsArray();
}
finalConditionsArray = finalConditions.GetParamsAsArray();
// check if a degenerated trajectory
var equal =
initialConditionsArray.Rank == finalConditionsArray.Rank &&
Enumerable.Range(0, initialConditionsArray.Rank).All(dimension => initialConditionsArray.GetLength(dimension) == finalConditionsArray.GetLength(dimension)) &&
initialConditionsArray.Cast <double>().SequenceEqual(finalConditionsArray.Cast <double>());
Matrix X1, Y1;
if (equal)
{
X1 = Matrix.Parse("0\r\n0\r\n0\r\n0");
Y1 = Matrix.Parse("0\r\n0\r\n0\r\n0");
}
else
{
X1 = CalcSpline3(new double[] { initialConditionsArray[0, 0], initialConditionsArray[0, 1] },
new double[] { finalConditionsArray[0, 0], finalConditionsArray[0, 1] },
time - startTime);
Y1 = CalcSpline3(new double[] { initialConditionsArray[1, 0], initialConditionsArray[1, 1] },
new double[] { finalConditionsArray[1, 0], finalConditionsArray[1, 1] },
time - startTime);
}
// calculate motion for start/resting position
for (double t = Ts; t <= finalTime + eps; t = t + Ts)
{
// Velocity
trajectory[1][0, counter] = Math.Round((X1.mat[0, 0] * 3 * t * t + X1.mat[1, 0] * 2 * t + X1.mat[2, 0]) * 1000) / 1000;
trajectory[1][1, counter] = Math.Round((Y1.mat[0, 0] * 3 * t * t + Y1.mat[1, 0] * 2 * t + Y1.mat[2, 0]) * 1000) / 1000;
// path
trajectory[0][0, counter] = X1.mat[0, 0] * t * t * t + X1.mat[1, 0] * t * t + X1.mat[2, 0] * t + X1.mat[3, 0];
trajectory[0][1, counter] = Y1.mat[0, 0] * t * t * t + Y1.mat[1, 0] * t * t + Y1.mat[2, 0] * t + Y1.mat[3, 0];
t = Math.Round(t * 1000) / 1000;
counter++;
}
return(trajectory);
}
catch (Exception ex)
{
return(null);
}
}
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);
}
