// This is the basic method used to move the arm.  The target position for each joint is specified along
        // with a time for the movement to be completed.  A port is returned which will receive a success message when the 
        // movement is completed or an exception message if an error is encountered.
        public SuccessFailurePort MoveTo(
            float baseVal,
            float shoulderVal,
            float elbowVal,
            float wristVal,
            float rotateVal,
            float gripperVal,
            float time)
        {
            var responsePort = new SuccessFailurePort();

            if (_moveToActive)
            {
                responsePort.Post(new Exception("Previous MoveTo still active."));
                return responsePort;
            }

            var values = new[] { baseVal, shoulderVal, elbowVal, wristVal, rotateVal, gripperVal};

            // check bounds.  If the target is invalid, post an exception message to the response port with a helpful error.
            for (int i = 0; i < _joints.Count; i++)
            {
                var val = values[i];
                if (!_joints[i].ValidTarget(val))
                {
                    responsePort.Post(new Exception(_joints[i].Name + "Joint set to invalid value: " + val));
                    return responsePort;
                }               
            }

/*
            if ((_joints[5].Target > gripperVal) && (Payload == null))
            {
                _attachPayload = true;
            }
            else if ((_joints[5].Target < gripperVal) && (Payload != null))
            {
                _dropPayload = true;
            }
*/

            // set the target values on the joint descriptors
            for (int i = 0; i < _joints.Count; i++)
                _joints[i].Target = values[i];

            // calculate a speed value for each joint that will cause it to complete its motion in the specified time
            for (int i = 0; i < _joints.Count; i++)
                _joints[i].Speed = Math.Abs(_joints[i].Target - _joints[i].Current) / time;

            // set this flag so that the motion is evaluated in the update method
            _moveToActive = true;

            // keep a pointer to the response port so we can post a result message to it.
            _moveToResponsePort = responsePort;

            return responsePort;
        }
Example #2
0
 private SuccessFailurePort UpdateState()
 {
     var resultPort = new SuccessFailurePort();
     //TODO: read state from service
     return resultPort;
 }
Example #3
0
        // This method calculates the joint angles necessary to place the arm into the 
        // specified position.  The arm position is specified by the X,Y,Z coordinates
        // of the gripper tip as well as the angle of the grip, the rotation of the grip, 
        // and the open distance of the grip.  The motion is completed in the 
        // specified time.
        public SuccessFailurePort MoveToPosition(
            float mx, // x position
            float my, // y position
            float mz, // z position
            float p, // angle of the grip
            float w, // rotation of the grip
            float grip, // distance the grip is open
            float time) // time to complete the movement
        {
            float baseAngle, shoulder, elbow, wrist;

            _moveTargetEntity.Position = new xna.Vector3(mx, my, mz);

            if (!_kinematics.InverseKinematics(mx, my, mz, p, out baseAngle, out shoulder, out elbow, out wrist))
            {
                var s = new SuccessFailurePort();
                s.Post(new Exception("Inverse Kinematics failed"));
                return s;
            }

            // Update the form with these parameters
            WinFormsServicePort.FormInvoke(() => _form.SetPositionText(mx, my, mz, p, w, grip, time));

            // Update the form with these parameters
            WinFormsServicePort.FormInvoke(() => _form.SetJointsText(baseAngle, shoulder, elbow, wrist, w, grip, time));

            var result = _robotArm.MoveTo(
                baseAngle,
                shoulder,
                elbow,
                wrist,
                w,
                grip,
                time);

            return result;
        }