public void JointTrackingFrameCaptured(IJointFrameParameter parameter)
{
if (parameter is AngularVelocityJointFrameParameter)
{
double angle = Math.Abs(parameter.BendAngle);
if (angle > Math.PI / 2)
{
smartPinMapping.CCwPinMapping.SetPin(0);
smartPinMapping.CwPinMapping.SetPin(255);
}
else
{
smartPinMapping.CwPinMapping.SetPin(0);
smartPinMapping.CCwPinMapping.SetPin(255);
}
}
else if (parameter is AngularDisplacementJointFrameParameter)
{
// Assume 200 steps per rotation.
// RBakerFlag -> TESTCODE!
if (parameter.TimeSpan.Equals(TimeSpan.Zero)) return;
int steps = Convert.ToInt32(200* (double)parameter.BendAngle / (2 * Math.PI));
int rpm = Convert.ToInt32(((double)steps / (double)200) / ((double)parameter.TimeSpan.Milliseconds / (1000 * 60)));
if (rpm > 60) rpm = 60;
byte[] commandPackage = new byte[] { (byte)CommandCodes.SetStepper1CommandCode, (byte)rpm, (byte)steps };
smartPinMapping.CwPinMapping.SetPinEventHandler(commandPackage);
}
}
private void AlertJoints(MovingJoint joint, DateTime timeStamp)
{
IJointFrameParameter parameter = GetJointFrameParameter(joint, timeStamp);
previousMovingJoint = joint;
previousTimeStamp = timeStamp;
Joints jointsToAlert = joints.GetJointsOfType(joint.JointType);
foreach (Joint jointToAlert in jointsToAlert)
{
jointToAlert.JointTrackingFrameCaptured(parameter);
}
}
public void JointTrackingFrameCaptured(IJointFrameParameter parameter)
{
if (parameter is AngularVelocityJointFrameParameter)
{
double angle = Math.Abs(parameter.BendAngle);
if (angle > Math.PI / 2)
{
smartPinMapping.CCwPinMapping.SetPin(0);
smartPinMapping.CwPinMapping.SetPin(255);
}
else
{
smartPinMapping.CwPinMapping.SetPin(0);
smartPinMapping.CCwPinMapping.SetPin(255);
}
}
else if (parameter is AngularDisplacementJointFrameParameter)
{
// Assume 200 steps per rotation.
// RBakerFlag -> TESTCODE!
if (parameter.TimeSpan.Equals(TimeSpan.Zero))
{
return;
}
int steps = Convert.ToInt32(200 * (double)parameter.BendAngle / (2 * Math.PI));
int rpm = Convert.ToInt32(((double)steps / (double)200) / ((double)parameter.TimeSpan.Milliseconds / (1000 * 60)));
if (rpm > 60)
{
rpm = 60;
}
byte[] commandPackage = new byte[] { (byte)CommandCodes.SetStepper1CommandCode, (byte)rpm, (byte)steps };
smartPinMapping.CwPinMapping.SetPinEventHandler(commandPackage);
}
}
