/// <summary>
/// たぶんあとで非同期待ち受けつかう
/// </summary>
/// <returns></returns>
IEnumerator ApplyPoseCoroutine()
{
float waitSec = 0.04f; //0.03だと送信失敗することがある
byte[] data1 = PreMaidUtility.BuildByteDataFromStringOrder("07 01 00 02 00 02 06");
_serialPort.Write(data1, 0, data1.Length);
yield return(new WaitForSeconds(waitSec));
byte[] data2 = PreMaidUtility.BuildByteDataFromStringOrder("07 01 00 08 00 02 0C");
_serialPort.Write(data2, 0, data2.Length);
yield return(new WaitForSeconds(waitSec));
byte[] data3 = PreMaidUtility.BuildByteDataFromStringOrder("08 02 00 08 00 FF FF 02");
_serialPort.Write(data3, 0, data3.Length);
yield return(new WaitForSeconds(waitSec));
byte[] data4 = PreMaidUtility.BuildByteDataFromStringOrder("04 04 00 00"); //フラッシュのライトプロテクト解除？
_serialPort.Write(data4, 0, data4.Length);
yield return(new WaitForSeconds(waitSec));
byte[] data5 = PreMaidUtility.BuildByteDataFromStringOrder("5c 1d 00 00 00"); //転送コマンド？
_serialPort.Write(data5, 0, data5.Length);
yield return(new WaitForSeconds(waitSec));
//ここでポーズ情報を取得する
byte[] data6 =
PreMaidUtility.BuildByteDataFromStringOrder(
BuildPoseString()); //対象のモーション、今回は1個だけ
_serialPort.Write(data6, 0, data6.Length);
yield return(new WaitForSeconds(waitSec * 2));
byte[] data7 = PreMaidUtility.BuildByteDataFromStringOrder("04 17 00 13 ff ff 41"); //不明
_serialPort.Write(data7, 0, data7.Length);
yield return(new WaitForSeconds(waitSec));
byte[] data8 = PreMaidUtility.BuildByteDataFromStringOrder("05 1E 00 01 1A");
_serialPort.Write(data8, 0, data8.Length);
yield return(new WaitForSeconds(waitSec));
byte[] data9 = PreMaidUtility.BuildByteDataFromStringOrder("05 1C 00 01 18"); //ベリファイダンプ要請
_serialPort.Write(data9, 0, data9.Length);
yield return(new WaitForSeconds(waitSec));
byte[] data10 = PreMaidUtility.BuildByteDataFromStringOrder("08 02 00 08 00 08 00 0A"); //モーションデータ転送終了
_serialPort.Write(data10, 0, data10.Length);
yield return(new WaitForSeconds(waitSec));
byte[] data11 = PreMaidUtility.BuildByteDataFromStringOrder("04 04 00 00"); //フラッシュのライトプロテクトを掛ける？
_serialPort.Write(data11, 0, data11.Length);
yield return(new WaitForSeconds(waitSec));
byte[] data12 = PreMaidUtility.BuildByteDataFromStringOrder("05 1F 00 01 1B"); //01番モーション再生
_serialPort.Write(data12, 0, data12.Length);
yield return(new WaitForSeconds(waitSec));
}
/// <summary>
/// 全サーボの強制脱力命令
/// </summary>
public void ForceAllServoStop()
{
//ここで連続送信モードを停止しないと、脱力後の急なサーボ命令で一気にプリメイドAIが暴れて死ぬ
SetContinuousMode(false);
string allStop = "50 18 00 06 02 00 00 03 00 00 04 00 00 05 00 00 06 00 00 07 00 00 08 00 00 09 00 00 0A 00 00 0B 00 00 0C 00 00 0D 00 00 0E 00 00 0F 00 00 10 00 00 11 00 00 12 00 00 13 00 00 14 00 00 15 00 00 16 00 00 17 00 00 18 00 00 1A 00 00 1C 00 00 FF";
byte[] allServoStopOrder = PreMaidUtility.BuildByteDataFromStringOrder(PreMaidUtility.RewriteXorString(allStop));
_serialPort.Write(allServoStopOrder, 0, allServoStopOrder.Length);
}
/// <summary>
/// たぶんあとで非同期待ち受けつかう
/// </summary>
/// <returns></returns>
IEnumerator ApplyPoseCoroutine()
{
float waitSec = 0.06f; //0.03だと送信失敗することがある
//ここでポーズ情報を取得する
byte[] willSendPoseBytes =
PreMaidUtility.BuildByteDataFromStringOrder(
BuildPoseString(80)); //対象のモーション、今回は1個だけ
_serialPort.Write(willSendPoseBytes, 0, willSendPoseBytes.Length);
yield return(new WaitForSeconds(waitSec));
}
/// <summary>
/// シリアルポート読み取り書き込みスレッド
/// 読みと書きを別スレッドにするより、まとめて1スレッドにしたほうがシリアルポートのlockが走らない分だけ早い
/// </summary>
private void ReadAndWriteThreadFunc()
{
Debug.LogWarning("シリアルポート送信スレッド起動");
var readBuffer = new byte[256 * 3];
var readCount = 0;
while (SerialPortOpen && _serialPort != null && _serialPort.IsOpen)
{
//PCから送る予定のキューが入っているかチェック
if (sendingQueue.IsEmpty == false)
{
var willSendString = string.Empty;
if (sendingQueue.TryDequeue(out willSendString))
{
byte[] willSendBytes =
PreMaidUtility.BuildByteDataFromStringOrder(willSendString);
_serialPort.Write(willSendBytes, 0, willSendBytes.Length);
}
}
//プリメイドAIからの受信チェック
try
{
readCount = _serialPort.Read(readBuffer, 0, readBuffer.Length);
if (readCount > 0)
{
receivedQueue.Enqueue(PreMaidUtility.DumpBytesToHexString(readBuffer, readCount));
}
}
catch (TimeoutException tEx)
{
//errorQueue.Enqueue("TimeOut Exception:" + tEx.Message);
//Thread.Sleep(1);
continue;
}
catch (System.Exception e)
{
errorQueue.Enqueue(e.Message);
//Debug.LogWarning(e.Message);
}
Thread.Sleep(1);
}
Debug.LogWarning("exit thread");
}
/// <summary>
/// シリアルポート読み取り書き込みスレッド
/// 読みと書きを別スレッドにするより、まとめて1スレッドにしたほうがシリアルポートのlockが走らない分だけ早い
/// </summary>
private void ReadAndWriteThreadFunc()
{
Debug.LogWarning("シリアルポート送信スレッド起動");
var readBuffer = new byte[256 * 3];
var readCount = 0;
var sendingCache = string.Empty; //送信失敗時に連続送信する
//バースト転送モード
bool burstMode = false;
while (SerialPortOpen && _serialPort != null && _serialPort.IsOpen)
{
//PCから送る予定のキューが入っているかチェック
if (sendingQueue.IsEmpty == false)
{
var willSendString = string.Empty;
if (sendingQueue.TryDequeue(out willSendString))
{
if (burstMode)
{
burstMode = false;
}
sendingCache = willSendString;
byte[] willSendBytes =
PreMaidUtility.BuildByteDataFromStringOrder(willSendString);
_serialPort.Write(willSendBytes, 0, willSendBytes.Length);
}
}
//プリメイドAIからの受信チェック
try
{
//本当はここのカウントもバッファ溜めつつ見た方が良い…
readCount = _serialPort.Read(readBuffer, 0, readBuffer.Length);
if (readCount > 0)
{
var receivedString = PreMaidUtility.DumpBytesToHexString(readBuffer, readCount);
//ポーズ送信失敗したらバーストモードに入る
if (receivedString.IndexOf("180814") >= 0)
{
burstMode = true;
}
if (receivedString.IndexOf("18001C") >= 0 && burstMode == true)
{
burstMode = false;
}
receivedQueue.Enqueue(receivedString);
}
}
//UnityのSerialPortの実装がタコなのでここでTimeout例外を握りつぶす必要があります
catch (TimeoutException tEx)
{
continue;
}
catch (System.Exception e)
{
errorQueue.Enqueue(e.Message);
//Debug.LogWarning(e.Message);
}
Thread.Sleep(1);
//送信失敗してた場合、無理矢理にキャッシュしてた最後のポーズ命令を連続送信する
//これで遅延を最小限にする
if (burstMode)
{
Thread.Sleep(5);
byte[] willSendBytes =
PreMaidUtility.BuildByteDataFromStringOrder(sendingCache);
_serialPort.Write(willSendBytes, 0, willSendBytes.Length);
}
}
Debug.LogWarning("exit thread");
}
/// <summary>
/// 足踏みから歩行へ至る処理を、とりあえずヤッツケで実装してみる。（直値埋め込みまくりで邪悪…）
/// </summary>
void MathWalk()
{
float X1 = Input.GetAxis("Horizontal");
float Y1 = Input.GetAxis("Vertical");
float X2 = Input.GetAxis("Horizontal2");
float Y2 = Input.GetAxis("Vertical2");
bool LT = Input.GetButton("LT");
int[] ServoVal = { 7500, 7500, 7500, 7500, 7500, 7500, 7500, 7500, 7500, 7500, 7500, 7500, 7500, 7500, 7500, 7500, 7500 };
// 姿勢制御（無くても歩ける）
if (Mathf.Abs(X2) > 0.1) // 腰ヨー軸もどき
{
ServoVal[12] += (int)(600 * X2);
ServoVal[13] += (int)(600 * X2);
ServoVal[14] += (int)(600 * X2); // 首回り
for (int i = 0; i < 3; i++)
{
ServoVal[i + 0] += (int)((140 * X2) * LinkDefPitch[i + 0]);
ServoVal[i + 4] -= (int)((140 * X2) * LinkDefPitch[i + 4]);
}
}
if (Mathf.Abs(Y2) > 0.1) // 前後姿勢制御
{
ServoVal[15] += (int)(300 * X2); // 首回り
ServoVal[16] -= (int)(300 * Y2);
if (Y2 < 0) // しゃがみ
{
for (int i = 0; i < 8; i++)
{
ServoVal[i] -= (int)((400 * Y2) * LinkDefLen[i]);
}
}
else // 前屈
{
ServoVal[0] += (int)(150 * Y2); // 直値 配列もつけてない。 適当…
ServoVal[4] -= (int)(150 * Y2);
ServoVal[3] -= (int)(450 * Y2);
ServoVal[7] += (int)(450 * Y2);
}
}
// 歩行制御（トリガ―で足踏み開始、半周期で操作可能になる ）
if (WalkingCondition == 0 && LT == true)
{
WalkingCondition = 1;
curTick = 0;
}
if (WalkingCondition > 0)
{
if (++curTick >= maxTick)
{
curTick = 0;
if (WalkingCondition == 1)
{
WalkingCondition = 2;
}
if (LT == false)
{
WalkingCondition = 3;
}
}
float fRateLen = table_sin3[curTick];
float fRateRoll = table_sin1[curTick];
float fRateP1 = table_plot1[curTick];
float fRateP2 = table_plot2[curTick];
int ratio = 2;
if (WalkingCondition == 1) // 歩き始めの足踏み
{
if (curTick < 5)
{
ratio = 1;
}
else
{
WalkingCondition = 2;
}
}
if (WalkingCondition == 3) // 歩き終わりの足踏み
{
if (curTick < 5)
{
ratio = 1;
}
else
{
WalkingCondition = 0;
}
}
// ROLL処理 ==========
for (int i = 8; i < 12; i++)
{
ServoVal[i] += (int)((fRateRoll * DuraRoll) * LinkDefRoll[i] * ratio);
}
// LEN処理 ==========
int tmp = 0;
if (fRateLen < 0) // 正負で上げる足を違える
{
tmp = 4;
fRateLen = -fRateLen;
}
for (int i = tmp; i < tmp + 4; i++)
{
ServoVal[i] += (int)((fRateLen * DuraLen) * LinkDefLen[i] * ratio);
}
if (WalkingCondition == 2) // 足踏み完了後(#2) STICK操作が可能になる
// Pitch処理 ==========
{
for (int i = 0; i < 4; i++)
{
ServoVal[i + 0] += (int)((fRateP1 * DuraPitch) * LinkDefPitch[i + 0] * Y1);
ServoVal[i + 4] += (int)((fRateP2 * DuraPitch) * LinkDefPitch[i + 4] * Y1);
}
// Yaw処理 ==========
if ((WalkingSteering != 0 || Mathf.Abs(X1) > 0.1))
{
int Steering = -(int)(X1 * 250);
if (curTick == 2 || curTick == 3 || curTick == 4)
{
if (Steering > 0)
{
WalkingSteering += Steering;
}
else
{
WalkingSteering = (curTick == 4) ? 0 : WalkingSteering / 2;
}
}
if (curTick == 7 || curTick == 8 || curTick == 9)
{
if (Steering < 0)
{
WalkingSteering -= Steering;
}
else
{
WalkingSteering = (curTick == 9) ? 0 : WalkingSteering / 2;
}
}
}
ServoVal[12] += WalkingSteering;
ServoVal[13] -= WalkingSteering;
}
}
// 0x18コマンドの完成
string cmd = "38 18 00 " + TickSpeed.ToString("X2");
for (int i = 0; i < 17; i++)
{
byte h = (byte)(ServoVal[i] / 256);
byte l = (byte)(ServoVal[i] % 256);
cmd += " " + ServoId[i].ToString("X02") + " " + l.ToString("X02") + " " + h.ToString("X02");
}
cmd += " 00"; // XOR予約 memo 長さ不足はエラー10
byte[] data = PreMaidUtility.BuildByteDataFromStringOrder(cmd); // IZMさんライブラリを使う場合はコウなる。
_serialPort.Write(data, 0, data.Length);
}