//---------------------------------------
// FUNCTIONS - UNITY3D (awake, start, update, reset)
//---------------------------------------
#region Unity3D Functions
#region Start
/// <summary>
/// Start function - called on start-up of program
/// </summary>
void Start()
{
// --------------------
// VULCANX COMMUNICATION, MPL HANDLERS
// --------------------
#region VulcanX Communication Initialization
GameObject VIESYSHandle = GameObject.Find("VIESYS");
VulcanXHandle = (VulcanXInterface)VIESYSHandle.GetComponent(typeof(VulcanXInterface));
#endregion //VulcanX Communication Initialization
f_HomePositionAngles = new float[MPL_NUM_ARM_JOINTS] {
0f, 0f, 0f, 0f, 0f, 0f, 0f
};
f_HomeFingerAngles = new float[MPL_NUM_FINGER_JOINTS] {
0f, 20f, 20f, 20f, 0f, 20f, 20f, 20f, 0f, 20f, 20f, 20f, 0f, 20f, 20f, 20f, 20f, 20f, 20f, 20f
};
// --------------------
// GAME ELEMENTS SETUP
// --------------------
#region Game Elements Setup
StartCoroutine(CommandRightMPLPosition(5, f_HomePositionAngles, f_HomeFingerAngles));
#endregion //Game Elements Setup
//Movement State (to Start)
SetMovementState(i_MovementState);
}//function - Start
private void MapDelegates()
{
switch (m_name)
{
case "rThDistal":
m_sliderGroupID = 0;
m_valAA = delegate() { return(VulcanXInterface.RightThumbAA()); };
m_valMCP = delegate() { return(VulcanXInterface.RightThumbFE()); };
m_valPIP = delegate() { return(VulcanXInterface.RightThumbMCP()); };
m_valDIP = delegate() { return(VulcanXInterface.RightThumbDIP()); };
break;
case "rIndDistal":
m_sliderGroupID = 1;
m_valAA = delegate() { return(VulcanXInterface.RightIndexAA()); };
m_valMCP = delegate() { return(VulcanXInterface.RightIndexMCP()); };
m_valPIP = delegate() { return(VulcanXInterface.RightIndexPIP()); };
m_valDIP = delegate() { return(VulcanXInterface.RightIndexDIP()); };
break;
case "rMidDistal":
m_sliderGroupID = 2;
m_valAA = delegate() { return(VulcanXInterface.RightMiddleAA()); };
m_valMCP = delegate() { return(VulcanXInterface.RightMiddleMCP()); };
m_valPIP = delegate() { return(VulcanXInterface.RightMiddlePIP()); };
m_valDIP = delegate() { return(VulcanXInterface.RightMiddleDIP()); };
break;
}
}
/// <summary>
/// This method is called several times to complete the reset process.
/// It uses yield to resume execution after two frames render. It then
/// sleeps for two seconds while the vMPL returns to its position prior
/// to the reset while blanking the cameras. Finally, it restores the
/// cameras.
/// </summary>
/// <returns></returns>
private IEnumerator FinishReset()
{
// Clear subscriber list now since transform updates don't need to
// be sent while waiting for reset.
// WorldInterface.Instance().ClearSubscribers();
// Wait 2 frames to ensure that objects are destroyed.
yield return(new WaitForEndOfFrame());
yield return(new WaitForEndOfFrame());
// It's safe to turn off the reset flag once old objects destroyed.
m_reset = false;
// Reset MPL via VulcanXInterface.
VulcanXInterface vx = gameObject.GetComponent <VulcanXInterface>();
if (vx == null)
{
throw new ApplicationException("VulcanXInterface not found.");
}
vx.Reset();
// Reset WorldInterface internals.
// WorldInterface wif = WorldInterface.Instance();
// wif.ClearWorldObjects();
// wif.AddWorldObjects();
// Disable game objects so nothing else moves when physics turned
// back on to move arm back to its pre-reset position.
DisableGameObjects();
// Blank cameras while arm moves.
BlankCameras();
// Start physics simulation.
Time.timeScale = 1;
// Give arm 2 seconds to move back to its position.
yield return(new WaitForSeconds(2));
// Pause simulation again and turn game objects back on.
// WorldInterface.Instance().Paused = true;
EnableGameObjects();
RestoreCameras();
// wif.ResetComplete();
}
void FixedUpdate()
{
lock (m_valLock)
{
//save actual joint values
m_actVal[0] = m_shFE.angle;
m_actVal[1] = m_shAA.angle;
m_actVal[2] = m_humRot.angle;
m_actVal[3] = m_elbFE.angle;
m_actVal[4] = m_wrRot.angle;
m_actVal[5] = m_wrDev.angle;
m_actVal[6] = m_wrFE.angle;
m_actVal[7] = m_finAA.angle;
m_actVal[8] = m_mcpFE.angle;
m_actVal[9] = m_proxFE.angle;
m_actVal[10] = m_medFE.angle;
//save commanded joint values
m_cmdVal[0] = VulcanXInterface.RightShoulderFE();
m_cmdVal[1] = VulcanXInterface.RightShoulderAA();
m_cmdVal[2] = VulcanXInterface.RightHumeralRot();
m_cmdVal[3] = VulcanXInterface.RightElbowFE();
m_cmdVal[4] = VulcanXInterface.RightWristRot();
m_cmdVal[5] = VulcanXInterface.RightWristDev();
m_cmdVal[6] = VulcanXInterface.RightWristFE();
m_cmdVal[7] = m_valAA();
m_cmdVal[8] = m_valMCP();
m_cmdVal[9] = m_valPIP();
m_cmdVal[10] = m_valDIP();
}
m_contactForce.Normalize();
// Unity 3.4+ call.
//Debug.DrawRay(transform.position, m_contactForce, Color.yellow, 0.25f);
//Debug.DrawRay(transform.position, m_contactForce*100, Color.yellow, 1.0f);
// Multiply by position error of joints.
// Send out summed force vector to VulcanXInterface.
// Zero force vector in preparation for collision events after the
// current time step.
//m_contactForce = Vector3.zero;
}
