Implement Arm goto() method based on attached pseudocode

[kalee1]

public class MotorArm
{
    boolean moving = false;
    boolean elbowDone = true;
    boolean shoulderDone = true;
    int shoulderInitPos = 0;
    int elbowInitPos = 0;

    public boolean goto( ArmPosEnum theTarget )
    {
        if not moving
        {
            shoulderInitPos = current shoulder position
            elbowInitPos = current elbow position

            if theTarget.elbow() - elbowInitPos > 0
            {
                set a positive number for elbow power
            }
            else
            {
                set a negative number for elbow power
            }

            if theTarget.shoulder() - shoulderInitPos > 0
            {
                set a positive number for shoulder power
            }
            else
            {
                set a negative number for shoulder power
            }

            set moving = true
            set elbow done flag = false
            set shoulder done flag = false
        }

        if elbow power is negative AND elbow current position is <= theTarget.elbow()
            OR
           elbow power is positive AND elbow current position is >= theTarget.elbow()
        {
            set elbow power value = 0
            set elbow done flag = true
        }

        if shoulder power is negative AND shoulder current position is <= theTarget.shoulder()
            OR
           shoulder power is positive AND shoulder current position is >= theTarget.shoulder()
        {
            set shoulder power value = 0
            set shoulder done flag  = true
        }

        if (shoulder done flag == false OR elbow done flag == false)
        {
            call armDrive with the shoulder power and elbow power
        }

        if ( shoulder done flag == true AND elbow done flag == true )
        {
            set moving = false
            call armDrive with 0 power for both shoulder and elbow
        }

        return !moving
    }

}