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)); }