public override void MotionGuidance()
{
double[] puckPnom, puckVnom;
double[] malletPnom, malletVnom;
double deltaP0x, deltaP0y, deltaPx, deltaPy;
double ux, uy, ux_prev, uy_prev;
double ex_prev, ey_prev, ex, ey;
ex_prev = 0;
ey_prev = 0;
ux_prev = 0;
uy_prev = 0;
Stopwatch planTime = new Stopwatch();
while (internalState == ModuleState.Active)
{
planTime.Restart();
puckPnom = puckNominalTrajectory.GetNext(QueueType.Position);
puckVnom = puckNominalTrajectory.GetNext(QueueType.Velocity);
malletPnom = commandsQueue.GetNext(QueueType.Position);
malletVnom = commandsQueue.GetNext(QueueType.Velocity);
physicalState = WM.GetPhysicalState();
deltaP0x = malletPnom[0] - puckPnom[0];
deltaP0y = malletPnom[1] - puckPnom[1];
deltaPx = physicalState["AgentX"] - physicalState["PuckX"];
deltaPy = physicalState["AgentY"] - physicalState["PuckY"];
ex = deltaP0x - deltaPx;
ey = deltaP0y - deltaPy;
ux = a * ex + b * ex_prev - ux_prev;
uy = a * ey + b * ey_prev - uy_prev;
communicator.SendMessage(Command.Message, DoubleToString(new double[2] {
ux, uy
}));
ex_prev = ex;
ey_prev = ey;
ux_prev = ux;
uy_prev = uy;
planTime.Stop();
if ((Ts - planTime.Elapsed).TotalMilliseconds > 0)
{
Thread.Sleep(Ts - planTime.Elapsed);
}
}
}
public override void MotionGuidance()
{
while (internalState == ModuleState.Active)
{
communicator.SendMessage(Command.Message, DoubleToString(commandsQueue.GetNext(QueueType.Velocity)));
Thread.Sleep(Ts);
}
}