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