/// <summary>
/// Initializes transition to new arm mode
/// </summary>
/// <param name="newMode"></param>
public void SetArmMode(ArmMode newMode)
{
IsTransitioning = true;
ArmMode oldMode = new ArmMode(
ValueHolder[(int)ArduinoValues.ArmRotator],
ValueHolder[(int)ArduinoValues.ArmShoulder],
ValueHolder[(int)ArduinoValues.ArmElbow],
ValueHolder[(int)ArduinoValues.ArmWrist],
ValueHolder[(int)ArduinoValues.ArmGrabber]
);
new Thread(() =>
{
TransitionArmMode(oldMode, newMode);
IsTransitioning = false;
}).Start();
}
/// <summary>
/// Transitions to new arm mode using recursion
/// </summary>
/// <param name="oldMode"></param>
/// <param name="newMode"></param>
/// <param name="currentTime"></param>
private void TransitionArmMode(ArmMode oldMode, ArmMode newMode, double currentTime = 0.0)
{
double transitionConstant = currentTime / Config.ModeTransitionDuration;
bool finalTransition = false;
if (transitionConstant > 1)
{
transitionConstant = 1;
finalTransition = true;
}
ValueHolder[(int)ArduinoValues.ArmRotator] = LinearTransition(transitionConstant, oldMode.RotatorAngle, newMode.RotatorAngle);
ValueHolder[(int)ArduinoValues.ArmShoulder] = LinearTransition(transitionConstant, oldMode.ShoulderAngle, newMode.ShoulderAngle);
ValueHolder[(int)ArduinoValues.ArmElbow] = LinearTransition(transitionConstant, oldMode.ElbowAngle, newMode.ElbowAngle);
ValueHolder[(int)ArduinoValues.ArmWrist] = LinearTransition(transitionConstant, oldMode.WristAngle, newMode.WristAngle);
ValueHolder[(int)ArduinoValues.ArmGrabber] = LinearTransition(transitionConstant, oldMode.GrabberAngle, newMode.GrabberAngle);
Thread.Sleep(Config.ModeTransitionDelay);
if (!finalTransition)
{
TransitionArmMode(oldMode, newMode, currentTime + Config.ModeTransitionDelay);
}
}
