private void OpenGripper()
{
Debug.WriteLine("Opening gripper.");
MoveLogicalCommand moveCommand = new MoveLogicalCommand(m_Servos);
moveCommand[ChannelId.Gripper] = 20;
m_SSC32Board.RunCommand(moveCommand);
}
private void CloseGripper()
{
Debug.WriteLine("Closing gripper.");
MoveLogicalCommand moveCommand = new MoveLogicalCommand(m_Servos);
moveCommand[ChannelId.Gripper] = c_ClosedGripperPosition;
this.SSC32Board.RunCommand(moveCommand);
}
private void MoveToHomePosition()
{
Debug.WriteLine("Moving to home position.");
MoveLogicalCommand moveCommand = new MoveLogicalCommand(m_Servos);
moveCommand[ChannelId.Base] = -90;
moveCommand[ChannelId.Shoulder] = 0;
moveCommand[ChannelId.Elbow] = 0;
moveCommand[ChannelId.WristUpDown] = -90;
moveCommand[ChannelId.Gripper] = 20;
m_SSC32Board.RunCommand(moveCommand);
}
private void MoveToDropTarget()
{
Debug.WriteLine("Moving to drop target.");
JointAngles jointAngles = GetJointAngles(m_DropOffLocation.X, m_DropOffLocation.Y, m_DropOffLocation.Z);
MoveLogicalCommand moveCommand = new MoveLogicalCommand(m_Servos);
moveCommand[ChannelId.Base] = jointAngles.BaseAngle;
moveCommand[ChannelId.Shoulder] = jointAngles.ShoulderAngle;
moveCommand[ChannelId.Elbow] = jointAngles.ElbowAngle;
moveCommand[ChannelId.WristUpDown] = jointAngles.WristTiltAngle;
this.SSC32Board.RunCommand(moveCommand);
}
private void MoveDownToPickupLocation(Vector3 pickupLocation)
{
Debug.WriteLine("Moving to pickup location.");
MoveLogicalCommand moveCommand = new MoveLogicalCommand(m_Servos);
JointAngles jointAngles = GetJointAngles(pickupLocation.X, pickupLocation.Y, pickupLocation.Z);
moveCommand[ChannelId.Base] = jointAngles.BaseAngle;
moveCommand[ChannelId.Shoulder] = jointAngles.ShoulderAngle;
moveCommand[ChannelId.Elbow] = jointAngles.ElbowAngle;
moveCommand[ChannelId.WristUpDown] = jointAngles.WristTiltAngle;
this.SSC32Board.RunCommand(moveCommand);
}
private void MoveToDropTarget()
{
Debug.WriteLine("Moving to drop target.");
Vector3 wristLocation = new Vector3(50, 150, 150);
JointAngles jointAngles = GetJointAngles(50, 150, 40, true);
MoveLogicalCommand moveCommand = new MoveLogicalCommand(m_Servos);
moveCommand[ChannelId.Base] = jointAngles.BaseAngle;
moveCommand[ChannelId.Shoulder] = jointAngles.ShoulderAngle;
moveCommand[ChannelId.Elbow] = jointAngles.ElbowAngle;
moveCommand[ChannelId.WristUpDown] = GetWristTiltAngleForVertical(jointAngles);
//moveCommand[ChannelId.Gripper] = 28;
m_SSC32Board.RunCommand(moveCommand);
}
private void MoveToPickupLocation()
{
Debug.WriteLine("Moving to pikup location.");
MoveLogicalCommand moveCommand = new MoveLogicalCommand(m_Servos);
JointAngles jointAngles = GetJointAngles(this.X, this.Y, this.Z, this.ZIsBasedOnEndeffectorTip);
moveCommand[ChannelId.Base] = jointAngles.BaseAngle;
moveCommand[ChannelId.Shoulder] = jointAngles.ShoulderAngle;
moveCommand[ChannelId.Elbow] = jointAngles.ElbowAngle;
moveCommand[ChannelId.WristUpDown] = GetWristTiltAngleForVertical(jointAngles);
//moveCommand[ChannelId.Gripper] = 20;
m_SSC32Board.RunCommand(moveCommand);
}
private void MoveAboveHomePosition()
{
Debug.WriteLine("Moving above home position.");
MoveLogicalCommand moveCommand = new MoveLogicalCommand(m_Servos);
JointAngles jointAngles = GetJointAngles(m_HomeLocation.X, m_HomeLocation.Y, m_HomeLocation.Z + 30);
moveCommand[ChannelId.Base] = jointAngles.BaseAngle;
moveCommand[ChannelId.Shoulder] = jointAngles.ShoulderAngle;
moveCommand[ChannelId.Elbow] = jointAngles.ElbowAngle;
moveCommand[ChannelId.WristUpDown] = jointAngles.WristTiltAngle;
moveCommand[ChannelId.Gripper] = c_OpenGripperPosition;
this.SSC32Board.RunCommand(moveCommand);
}
private void MoveTo(Vector3 location, string message)
{
if (!String.IsNullOrEmpty(message))
{
Debug.WriteLine(message);
}
MoveLogicalCommand moveCommand = new MoveLogicalCommand(m_Servos);
JointAngles jointAngles = GetJointAngles(location.X, location.Y, location.Z);
moveCommand[ChannelId.Base] = jointAngles.BaseAngle;
moveCommand[ChannelId.Shoulder] = jointAngles.ShoulderAngle;
moveCommand[ChannelId.Elbow] = jointAngles.ElbowAngle;
moveCommand[ChannelId.WristUpDown] = jointAngles.WristTiltAngle;
this.SSC32Board.RunCommand(moveCommand);
}
