// 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;
}
private SuccessFailurePort UpdateState()
{
var resultPort = new SuccessFailurePort();
//TODO: read state from service
return resultPort;
}
// 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;
}
