/// <summary> /// 現在のAnimatorについているサーボの値を参照しながら差分だけ送る /// </summary> void ApplyMecanimPoseWithDiff() { var servos = _controller.Servos; List <PreMaidServo> orders = new List <PreMaidServo>(); foreach (var VARIABLE in _joints) { var mecanimServoValue = VARIABLE.currentServoValue; ServoPosition targetServoID = (ServoPosition)VARIABLE.servoNo; var servo = servos.Find(x => x.ServoPositionEnum == targetServoID); int premaidServoValue = servo.GetServoValue(); // mecanimServoValueがゼロなのは異常 if (mecanimServoValue <= 0) { Debug.Log(string.Format("Servo:{0} Val:{1}", servo.GetServoName(), mecanimServoValue - premaidServoValue)); continue; } //閾値よりサーボの値が変わってたら命令とする //50とかでもいいかも if (Mathf.Abs(mecanimServoValue - premaidServoValue) > 10) { servo.SetServoValueSafeClamp((int)mecanimServoValue); PreMaidServo tmp = new PreMaidServo(targetServoID); tmp.SetServoValueSafeClamp((int)mecanimServoValue); orders.Add(tmp); } } //ここでordersに差分だけ送れます coolTime = orders.Count * 0.005f; //25個あると0.08くらい、1個だと0.01くらいのクールタイムが良い if (orders.Count > 0) { currentFPS++; //Debug.Log("Servo Num:" + orders.Count); _controller.ApplyPoseFromServos(orders, Mathf.Clamp(orders.Count * 2, 10, 40)); } if (_initialized == false) { _initialized = true; } }
/// <summary> /// 現在のAnimatorについているサーボの値を参照しながら全て送る /// </summary> void ApplyMecanimPoseAll() { var servos = _controller.Servos; List <PreMaidServo> orders = new List <PreMaidServo>(); foreach (var VARIABLE in _joints) { var mecanimServoValue = VARIABLE.currentServoValue; ServoPosition targetServoID = (ServoPosition)VARIABLE.servoNo; var servo = servos.Find(x => x.ServoPositionEnum == targetServoID); int premaidServoValue = servo.GetServoValue(); // mecanimServoValueがゼロなのは異常 if (mecanimServoValue <= 0) { Debug.Log(string.Format("Servo:{0} Val:{1}", servo.GetServoName(), mecanimServoValue - premaidServoValue)); continue; } servo.SetServoValueSafeClamp((int)mecanimServoValue); PreMaidServo tmp = new PreMaidServo(targetServoID); tmp.SetServoValueSafeClamp((int)mecanimServoValue); orders.Add(tmp); } //ここでordersに差分だけ送れます。speed=40でcooltime=0.05fでいけた!?!? //つまり20FPSだとたまに送信失敗するけど意外と通る。 //BT環境が悪かったらもっと速度を落とすとか? //cooltime=0.08f だと12FPS送信になって結構失敗しないです coolTime = 0.05f; keyFrameTimer = 1f; //Debug.Log("全フレーム転送 :" + orders.Count+" FPS:"+currentFPS); _controller.ApplyPoseFromServos(orders, 40); currentFPS = 0; if (_initialized == false) { _initialized = true; } }
/// <summary> /// サーボ情報一つ分を記憶 /// </summary> /// <param name="servoNo"></param> void AddServo(ServoPosition servoNo) { var joint = ModelJoint.GetJointById((int)servoNo, ref _joints); if (joint) { servos.Add(servoNo, joint); // 開発用に、いったん制限なしとする joint.minAngle = -179f; joint.maxAngle = 180f; if (joint.minAngle > 0f) { joint.minAngle = 0f; // 開発用。Tポーズだと上腕が0degのため } if (joint.maxAngle < 0f) { joint.maxAngle = 0f; // 開発用。Tポーズだと上腕が0degのため } } //Debug.Log(servoId + " " + joint.name); }
public PreMaidServo(ServoPosition servoPosition) { _defaultServoValue = 7500; _servoPosition = servoPosition; servoName = servoPosition.ToString(); //TODO:このServoのリミット設定は、大雑把に安全そうなリミットを設定しています。 //なので、場合によってはこの値を緩められるか検討するのは可能です。 //4000-11000 //肩回りShoulderPitchは配線の関係で腕を後ろに回すと断線する恐れがあります。 switch (_servoPosition) { case ServoPosition.RightShoulderPitch: MaxServoValue = 7500; //手を下ろした状態 MinServoValue = 2500; //バンザイ break; case ServoPosition.HeadPitch: MaxServoValue = 8000; //うなづいた感じ MinServoValue = 7300; //上向いた限界 break; case ServoPosition.LeftShoulderPitch: MaxServoValue = 12500; //バンザイ MinServoValue = 7500; ///手を下ろした状態 break; case ServoPosition.HeadYaw: MinServoValue = 6600; //左を向いてる MaxServoValue = 8400; //右を向いてる break; case ServoPosition.RightHipYaw: MaxServoValue = 7900; //内また気味 MinServoValue = 7000; //外開き break; case ServoPosition.HeadRoll: MaxServoValue = 7800; //左に首をかしげる MinServoValue = 7200; //右に首をかしげる break; case ServoPosition.LeftHipYaw: MinServoValue = 7000; //内また MaxServoValue = 8000; //外開き break; case ServoPosition.RightShoulderRoll: SetServoValueSafeClamp("1C 25"); //9500 MinServoValue = 7500; //Tポーズ MaxServoValue = 9700; //気を付け break; case ServoPosition.RightHipRoll: MinServoValue = 7000; // やすめ、肩幅に開く MaxServoValue = 7700; // 気を付け break; case ServoPosition.LeftShoulderRoll: SetServoValueSafeClamp("7C 15"); //5500 MinServoValue = 5200; //気を付け MaxServoValue = 7300; //Tポーズ break; case ServoPosition.LeftHipRoll: MaxServoValue = 8000; // やすめ、肩幅に開く MinServoValue = 7300; // 気を付け break; case ServoPosition.RightUpperArmYaw: MinServoValue = 5000; //手の外ひねり MaxServoValue = 8000; //手の内ひねり break; case ServoPosition.RightUpperLegPitch: //シビアですぐこけるよ!! MaxServoValue = 9000; //のけぞる MinServoValue = 6000; //前屈 break; case ServoPosition.LeftUpperArmYaw: MinServoValue = 7000; //手の内ひねり MaxServoValue = 10000; //手の外ひねり break; case ServoPosition.LeftUpperLegPitch: //シビアですぐこけるよ!! MaxServoValue = 9000; //前屈 MinServoValue = 6000; //のけぞる break; case ServoPosition.RightLowerArmPitch: MaxServoValue = 11000; //肘を力こぶ出来る方に曲げる10000とかいけるけど、体に刺さらないように要調整(ほかの腕軸を見てね) MinServoValue = 7400; //肘を外開き、無理させないでね、もげるよ break; case ServoPosition.RightLowerLegPitch: MinServoValue = 5000; //膝を曲げる。当然5000とかにするとコケる MaxServoValue = 7550; //膝をあり得ない方に曲げる。ほとんど曲がらない break; case ServoPosition.LeftLowerArmPitch: MaxServoValue = 7600; //肘を外開き、無理させないでね、もげるよ MinServoValue = 4000; //肘を力こぶ出来る方に曲げる 5000とかいけるけど、体に刺さらないように要調整(ほかの腕軸を見てね) break; case ServoPosition.LeftLowerLegPitch: MinServoValue = 7450; //膝をあり得ない方に曲げる。ほとんど曲がらない MaxServoValue = 9000; //膝を曲げる。当然10000とかにするとコケる break; case ServoPosition.RightHandYaw: MinServoValue = 6000; //手のひらを外回し MaxServoValue = 9000; //手のひらを内回し break; case ServoPosition.RightFootPitch: break; case ServoPosition.LeftHandYaw: break; case ServoPosition.LeftFootPitch: break; case ServoPosition.RightFootRoll: break; case ServoPosition.LeftFootRoll: break; default: throw new ArgumentOutOfRangeException(nameof(servoPosition), servoPosition, null); } }
private void ServoPositionSlider_Changed(object sender, RangeBaseValueChangedEventArgs e) { _servo.SetPosition((int)e.NewValue).AllowTimeToMove(100).Go(); ServoPosition = (int)e.NewValue; TxtServoPosition.Text = ServoPosition.ToString(CultureInfo.InvariantCulture); }