public OpenLoopMotionGuidanceStrategy(TrajectoryQueue queue, Communicator.Communicator com, int timeCycle)
{
communicator = com;
commandsQueue = queue;
Ts = TimeSpan.FromMilliseconds(timeCycle);
internalState = ModuleState.Inactive;
}
public PDMotionGuidanceStrategy(TrajectoryQueue queue, Communicator.Communicator com, double timeCycle, WorldModel worldModel)
{
communicator = com;
commandsQueue = queue;
Ts = TimeSpan.FromSeconds(timeCycle);
internalState = ModuleState.Inactive;
WM = worldModel;
puckNominalTrajectory = WM.GetPuckTrajectory();
Kp = 0.5;
//Kp = 0.8;
Ki = 22.125;
//Ki = 10;
double T = Ts.TotalSeconds;
a = (2 * Ki + T * Kp) / 2;
b = (2 * Ki - T * Kp) / 2;
}
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 override SenseEventType Estimate(double agentX, double agentY, double agentVx, double agentVy,
double puckX, double puckY, double puckVx, double puckVy, double PuckR,
double oppX, double oppY, double oppVx, double oppVy, WorldModel worldModel)
{
Dictionary <string, double> oldState = worldModel.GetPhysicalState();
SenseEventType event2return = SenseEventType.NoEvent;
Point puckPold = new Point(oldState["PuckX"], oldState["PuckY"]);
Point puckPnew = new Point(puckX, puckY);
Point puckVold = new Point(oldState["PuckVx"], oldState["PuckVy"]);
Point puckVnew = new Point(puckVx, puckVy);
Point oppPold = new Point(oldState["OpponentX"], oldState["OpponentY"]);
Point oppPnew = new Point(oppX, oppY);
Point agentPold = new Point(oldState["AgentX"], oldState["AgentY"]);
Point agentPnew = new Point(agentX, agentY);
worldModel.UpdatePhysicalState(agentX, agentY, agentVx, agentVy, puckX, puckY, puckVx, puckVy, PuckR,
oppX, oppY, oppVx, oppVy);
if (VelocityChanged(puckVold, puckVnew))
{
if ((puckPold.Dist(agentPold) < 80 + eps) || (puckPnew.Dist(agentPnew) < 80 + eps))
{
event2return = SenseEventType.AgentCollision; // agent gave a hit
}
else if ((puckPold.Dist(oppPold) < 80 + eps) || (puckPnew.Dist(oppPnew) < 80 + eps))
{
event2return = SenseEventType.OpponentCollision; // player gave a hit
}
else if (Math.Sign(puckVold.Y) == -Math.Sign(puckVnew.Y)) // Vy_old == -Vy_new
{
if (IsZero(puckVnew.X)) // puck is stuck on a line (bouncing from side to side)
{
if (puckPnew.X < -eps)
{
event2return = SenseEventType.StuckAgent; // stuck on the agent's side
}
else
{
event2return = SenseEventType.StuckPlayer; // stuck on the player's side
}
}
else
{
event2return = SenseEventType.yWall; // hit one of the side walls
}
}
else if ((IsZero(puckVnew.X)) && (IsZero(puckVnew.Y))) // stuck (standing still) somewhere
{
if (puckPnew.X < -eps)
{
event2return = SenseEventType.StuckAgent; // standing still on the agent's side
}
else
{
event2return = SenseEventType.StuckPlayer; // stading still on the player's side
}
}
else if (Math.Sign(puckVold.X) != Math.Sign(puckVnew.X))
{
event2return = SenseEventType.xWall; // hit one of the back/front walls
}
else
{
event2return = SenseEventType.Disturbance;
}
}
else
{
event2return = SenseEventType.NoEvent;
}
TrajectoryQueue puckTrajectory = EstimateLineCrossing.EstimatePuckTrajectory(puckPnew, puckVnew, (double)global["PuckRadius"],
(int)global["Tablewidth"], (int)global["Tableheight"], (double)global["TimeStep"], (double)global["TimeScale"]);
worldModel.UpdatePuckTrajectory(puckTrajectory);
return(event2return);
}
