void Update()
{
_initialized = true;
if (Input.GetKey(KeyCode.Escape))
{
Application.Quit();
}
if (Input.GetKey(KeyCode.RightArrow))
{
transform.Rotate(new Vector3(0, -_speed, 0));
_streamClient.SetCachedServoPosition(ServoServerClient.ServoPortEnum.D1, _streamClient.MapToByte(transform.rotation.x));
}
if (Input.GetKey(KeyCode.LeftArrow))
{
transform.Rotate(new Vector3(0, _speed, 0));
_streamClient.SetCachedServoPosition(ServoServerClient.ServoPortEnum.D1, _streamClient.MapToByte(transform.rotation.x));
}
if (Input.GetKey(KeyCode.DownArrow))
{
transform.Rotate(new Vector3(_speed, 0, 0));
_streamClient.SetCachedServoPosition(ServoServerClient.ServoPortEnum.D0, _streamClient.MapToByte(transform.rotation.y));
}
if (Input.GetKey(KeyCode.UpArrow))
{
transform.Rotate(new Vector3(-_speed, 0, 0));
_streamClient.SetCachedServoPosition(ServoServerClient.ServoPortEnum.D0, _streamClient.MapToByte(transform.rotation.y));
}
// This will get the Rotations of the attached joints and will send them to the EZ-B ports D0, D1, D2
// Extract the Z rotation of this joint
BioJoint joint_D0 = D0.GetComponent <BioJoint>();
double value_D0 = joint_D0.Z.GetTargetValue();
int position_D0 = Mathf.RoundToInt((float)value_D0);
int positionAbs_D0 = Mathf.Abs(position_D0 - 90);
//print(positionAbs_D0);
// Extract the Y rotation of this joint
BioJoint joint_D1 = D1.GetComponent <BioJoint>();
double value_D1 = joint_D1.Y.GetTargetValue();
int position_D1 = Mathf.RoundToInt((float)value_D1);
int positionAbs_D1 = Mathf.Abs(180 - (position_D1 - 90));
//print(positionAbs_D1);
// Extract the Y rotation of this joint
BioJoint joint_D2 = D2.GetComponent <BioJoint>();
double value_D2 = joint_D2.Y.GetTargetValue();
int position_D2 = Mathf.RoundToInt((float)value_D2);
int positionAbs_D2 = Mathf.Abs(position_D2 - 90);
//print(positionAbs_D2);
_streamClient.SetCachedServoPosition(ServoServerClient.ServoPortEnum.D0, (byte)positionAbs_D0);
_streamClient.SetCachedServoPosition(ServoServerClient.ServoPortEnum.D1, (byte)positionAbs_D1);
_streamClient.SetCachedServoPosition(ServoServerClient.ServoPortEnum.D2, (byte)positionAbs_D2);
// Send all the servo positions if there's been a change
if (_streamClient.HasServoChanged)
{
_streamClient.SendCachedServoPositions();
}
if (_toDisplay.Length > 0)
{
_texture.LoadImage(_toDisplay);
var material = GetComponent <Renderer>().material;
material.mainTexture = _texture;
material.mainTextureScale = new Vector2(1, -1);
}
}
private void Awake()
{
joint = GetComponent <BioJoint>();
_root = transform.root;
threshold = 0.1f;
/*
* Label the joints based one their names.
* isLeft = [-1,0,1]
* chainPos = [-1,0,1] , shoulder and thighs are assigned to 1, hands and feet are assigned to -1, rest is 0
*/
if (gameObject.name.Contains("left") ||
gameObject.name.Contains("Left") ||
gameObject.name.Contains("_l"))
{
isLeft = 1;
}
else if (gameObject.name.Contains("right") ||
gameObject.name.Contains("Right") ||
gameObject.name.Contains("_r"))
{
isLeft = -1;
}
else
{
isLeft = 0;
}
if (gameObject.name.Contains("shoulder") ||
gameObject.name.Contains("Shoulder") ||
gameObject.name.Contains("Thigh") ||
gameObject.name.Contains("thigh") ||
gameObject.name.Contains("UpLeg"))
{
chainPos = 1;
}
else if (gameObject.name.Contains("hand") ||
gameObject.name.Contains("Hand") ||
gameObject.name.Contains("Wrist") ||
gameObject.name.Contains("Ankle") ||
gameObject.name.Contains("Foot") ||
gameObject.name.Contains("foot"))
{
chainPos = -1;
}
else
{
chainPos = 0;
}
centerOfMass = _root.InverseTransformPoint(transform.position);
totalMass = 1000 * math.PI * math.pow(radius, 2) * length.magnitude;
inertiaObj = new float3x3(
new float3(ReturnInertia(0), 0, 0),
new float3(0, ReturnInertia(1), 0),
new float3(0, 0, ReturnInertia(2))
);
initPosition = _root.InverseTransformPoint(transform.position);
initRotation = _root.transform.rotation * transform.rotation;
initRotationEuler = _root.InverseTransformDirection(transform.rotation.eulerAngles);
initOrientation = new float3x3(
_root.InverseTransformDirection(transform.forward),
_root.InverseTransformDirection(transform.up),
_root.InverseTransformDirection(transform.right));
}
