public void Update(double Seconds)
{
Program.Echo('\n' + "Update()");
LeftLock = (Left.Status == LegStatus.Lowered);
RightLock = (Right.Status == LegStatus.Lowered);
if (LastCommand != Command)
{
switch (Command)
{
case LegCommand.Stop:
Left.Stop();
Right.Stop();
break;
case LegCommand.Forward:
Left.Forward();
Right.Forward();
break;
case LegCommand.Backward:
Left.Backward();
Right.Backward();
break;
case LegCommand.Left:
Left.Backward();
Right.Forward();
break;
case LegCommand.Right:
Left.Forward();
Right.Backward();
break;
}
}
if (MovingLeg == null || LockedLeg == null)
{
if (LeftLock && RightLock)
{
if (NextUnlockRight || (Left.StridePercent < Right.StridePercent && !NextUnlockLeft))
{
MovingLeg = Right;
LockedLeg = Left;
}
else
{
MovingLeg = Left;
LockedLeg = Right;
}
MovingLeg.Raise();
}
else if (LeftLock)
{
MovingLeg = Right;
LockedLeg = Left;
}
else if (RightLock)
{
MovingLeg = Left;
LockedLeg = Right;
}
else
{
if (Preferance == Base6Directions.Direction.Left)
{
MovingLeg = Right;
LockedLeg = Left;
}
else
{
MovingLeg = Left;
LockedLeg = Right;
}
}
}
if (MovingLeg == Left)
{
Program.Echo("ML, SR");
}
else
{
Program.Echo("MR, SL");
}
if (NextLockLeft)
{
Program.Echo("NextLockLeft");
}
else if (NextLockRight)
{
Program.Echo("NextLockRight");
}
else
{
Program.Echo("NextLockNone");
}
if (Command != LegCommand.Stop)
{
//MovingLeg.Continue();
if (LockedLeg.State != LegState.Pushing)
{
LockedLeg.Push();
}
if (MovingLeg.State != LegState.Returning && MovingLeg.State != LegState.Lowering)
{
MovingLeg.Return();
}
else
{
if (MovingLeg.StridePercent < 50)
{
if (MovingLeg.State != LegState.Returning)
{
MovingLeg.Return();
if (MovingLeg == Left)
{
NextLockLeft = true;
}
else
{
NextLockRight = true;
}
}
}
else if (MovingLeg.StridePercent >= 50)
{
if (MovingLeg.State != LegState.Lowering)
{
MovingLeg.Drop();
if (MovingLeg == Left)
{
NextUnlockRight = true;
}
else
{
NextUnlockLeft = true;
}
}
}
if (MovingLeg.Status == LegStatus.Raised)
{
MovingLeg.Lower();
}
}
LastSwitch += Seconds;
}
else
{
MovingLeg.Stop();
LockedLeg.Stop();
}
LockedLeg.Update();
MovingLeg.Update();
LastCommand = Command;
if (LeftLock && RightLock && LastSwitch > 0.5 || LastSwitch > 5)
{
Leg temp = MovingLeg;
MovingLeg = LockedLeg;
LockedLeg = temp;
MovingLeg.Raise();
LastSwitch = 0;
}
}