/// <summary>
/// 指定されたサーボ値を適用してシリアル通信でプリメイドAI実機に送る
/// </summary>
public void ApplyPoseFromServos(IEnumerable <PreMaidServo> servos, int speed = 10)
{
if (SerialPortOpen == false)
{
Debug.LogWarning("ポーズ指定されたときにシリアルポートが開いていません");
return;
}
speed = Mathf.Clamp(speed, 1, 255);
int servoNum = servos.Count();
int orderLen = servoNum * 3 + 5; //命令長はサーボ個数が1個だったら0x08, サーボ個数が25個だったら0x50(80)になる
//決め打ちのポーズ命令+スピード(小さい方が速くて、255が最大に遅い)
string ret = orderLen.ToString("X2") + " 18 00 " + speed.ToString("X2");
//そして各サーボぼ値を入れる
foreach (var VARIABLE in servos)
{
ret += " " + VARIABLE.GetServoIdAndValueString();
}
ret += " FF"; //パリティビットを仮で挿入する;
//パリティビットを計算し直した値にして、文字列を返す
ret = PreMaidUtility.RewriteXorString(ret);
sendingQueue.Enqueue(ret); //対象のモーション、今回は1個だけ;
}
/// <summary>
/// バッテリー残量の問い合わせ、ハンドリングはPreMaidReceiver.csで行っています
/// </summary>
public void RequestBatteryRemain()
{
string batteryRequestOrder = "07 01 00 02 00 02 06";
//Debug.Log("リクエスト:"+ batteryRequestOrder);
sendingQueue.Enqueue(PreMaidUtility.RewriteXorString(batteryRequestOrder)); //バッテリー残量を教えてもらう
}
/// <summary>
/// 現在のサーボ値を適用する1フレームだけのモーションを送る
/// </summary>
/// <returns></returns>
string BuildPoseString(int speed = 50)
{
if (speed > 255)
{
speed = 255;
}
if (speed < 1)
{
speed = 1;
}
//決め打ちのポーズ命令+スピード(小さい方が速くて、255が最大に遅い)
string ret = "50 18 00 " + speed.ToString("X2");
//そして各サーボぼ値を入れる
foreach (var VARIABLE in Servos)
{
ret += " " + VARIABLE.GetServoIdAndValueString();
}
ret += " FF"; //パリティビットを仮で挿入する;
//パリティビットを計算し直した値にして、文字列を返す
return(PreMaidUtility.RewriteXorString(ret));
}
/// <summary>
/// たぶんこれでFLASHのダンプが返ってくる
/// </summary>
/// <param name="page"></param>
public void RequestFlashRomDump(int page)
{
string flashDump = "05 1C 00 " + string.Format("{0:X2}", page) + " FF";
Debug.Log("リクエスト:" + flashDump);
sendingQueue.Enqueue(PreMaidUtility.RewriteXorString(flashDump)); //FLASHの中身を教えてもらう？
}
/// <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>
/// "4C 1D"などの外部からサーボの値を入れる
/// ちなみに4C 1Dが7500です
/// </summary>
/// <param name="newValue">"4C 1D"</param>
public void SetServoValueSafeClamp(string spaceSplitedByteString)
{
var aaa = PreMaidUtility.ConvertEndian(PreMaidUtility.RemoveWhitespace(spaceSplitedByteString));
int intValue = int.Parse(aaa, System.Globalization.NumberStyles.HexNumber);
//Debug.Log($"{spaceSplitedByteString} は {intValue} ");
SetServoValueSafeClamp(intValue);
}
/// <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>
/// 全サーボの強制脱力命令
/// </summary>
public void ForceAllServoStop(bool disconnect = true)
{
//ここで連続送信モードを停止しないと、脱力後の急なサーボ命令で一気にプリメイドAIが暴れて死ぬ
//なので、普段はついでにシリアルポートも切る
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";
sendingQueue.Enqueue(PreMaidUtility.RewriteXorString(allStop)); //ストップ命令を送る
if (disconnect)
{
CloseSerialPort();
}
}
/// <summary>
/// 受信時の処理
/// </summary>
/// <param name="receivedString"></param>
/// <exception cref="NotImplementedException"></exception>
private void OnReceivedFromPreMaidAi(string receivedString)
{
//4文字以下なら不正
if (receivedString.Length < 4)
{
return;
}
//3-4文字目が命令種類
string orderKind = receivedString.Substring(2, 2);
//Debug.Log("orderKind:"+ orderKind);
switch (orderKind)
{
//バッテリー残量
case "01":
if (receivedString.Length >= 10)
{
int rawValtageValue =
PreMaidUtility.HexStringToInt(PreMaidUtility.ConvertEndian(receivedString.Substring(6, 4)));
Debug.Log($"バッテリー残量{rawValtageValue} で電圧は{rawValtageValue / 216f} V");
if (rawValtageValue / 216.0f < 9f)
{
Debug.LogError("バッテリー残量が9V以下です！！！！");
}
}
break;
//モーション転送結果
case "18":
if (receivedString == "0418001C")
{
}
else
{
Debug.Log("PoseError:" + receivedString);
}
break;
default:
Debug.Log(receivedString);
break;
}
}
/// <summary>
/// 現在のサーボ値を適用する1フレームだけのモーションを送る
/// </summary>
/// <returns></returns>
string BuildPoseString(int speed = 10)
{
speed = Mathf.Clamp(speed, 1, 255);
//決め打ちのポーズ命令+スピード(小さい方が速くて、255が最大に遅い)
string ret = "08 18 00 " + speed.ToString("X2");
//そして各サーボぼ値を入れる
var index = _servos.FindIndex(x => x.GetServoId() == 2);
ret += " " + _servos[index].GetServoIdAndValueString();
ret += " FF"; //パリティビットを仮で挿入する;
//パリティビットを計算し直した値にして、文字列を返す
return(PreMaidUtility.RewriteXorString(ret));
}
/// <summary>
/// 全サーボのスピードパラメータ指定命令
/// 18 19 10 10 3C 18 3C 1C 3C 14 3C 0C 3C 0E 3C 16 3C 1A 3C 12 3C 0A 3C 07
/// </summary>
public void ForceAllServoSpeedProperty(int speed)
{
var targetSpeed = Mathf.Clamp(speed, 1, 127);
string speedProp = string.Format("{0:X2}", targetSpeed);
//0x36=54個なので
//最初の3個+ 25サーボ*2パラメータ+ チェックバイト
string allServo =
"36 19 00";
foreach (var VARIABLE in Servos)
{
allServo += " " + VARIABLE.GetServoIdString() + " " + speedProp;
}
allServo += "FF";
sendingQueue.Enqueue(PreMaidUtility.RewriteXorString(allServo)); //ストレッチ命令を送る
}
/// <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>
/// 7500だったら"4C 1D"が返ってくる
/// </summary>
/// <returns></returns>
public string GetServoValueString()
{
var tmp = PreMaidUtility.ConvertEndian(_servoValue.ToString("X2"));
return($"{tmp[0]}{tmp[1]} {tmp[2]}{tmp[3]}");
}
// Update is called once per frame
void Update()
{
if (errorQueue.IsEmpty == false)
{
var errorString = string.Empty;
if (errorQueue.TryDequeue(out errorString))
{
Debug.LogError(errorString);
}
}
if (SerialPortOpen == false)
{
return;
}
//受信バッファ、バイナリで届くので区切りをどうしようか悩み中
//一旦、素朴に先頭に命令長が来るでしょう、というつもりで書きます。
if (receivedQueue.IsEmpty == false)
{
var receivedString = string.Empty;
if (receivedQueue.TryDequeue(out receivedString))
{
bufferedString += receivedString;
if (bufferedString.Length < 2)
{
return;
}
//異様にバッファが溜まったら捨てる
if (bufferedString.Length > 100)
{
Debug.Log("破棄します:" + bufferedString);
bufferedString = string.Empty;
return;
}
int orderLength = PreMaidUtility.HexStringToInt(bufferedString.Substring(0, 2));
//先頭0だったら命令ではないと判断して2文字読み捨て
//なぜなら0004051Fみたいな文字列が入っているので
if (orderLength == 0)
{
bufferedString = bufferedString.Substring(2);
}
//命令長が足りないので待つ
else if (orderLength > bufferedString.Length * 2)
{
return;
}
else if (bufferedString.Length >= orderLength * 2)
{
var targetOrder = bufferedString.Substring(0, orderLength * 2);
if (OnReceivedFromPreMaidAI != null)
{
OnReceivedFromPreMaidAI.Invoke(targetOrder);
}
else
{
Debug.Log(targetOrder);
}
//まだ余りバッファが有るならツメます
if (orderLength * 2 < bufferedString.Length)
{
bufferedString = bufferedString.Substring(orderLength * 2 + 1);
}
else
{
bufferedString = string.Empty;
}
}
}
}
}
/// <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);
}
