protected virtual void MultiTargetMoveController(float elipsed)
{
var cmd = motorMultiTargetCmd;
float translate = 0, rotate = 0;
// ---- Timeout ----
byte timeout = cmd.timeOut == 0? (byte)10:cmd.timeOut;
if (cmd.elipsed > timeout)
{
this.multiTargetMoveCallback?.Invoke(cmd.configID, Cube.TargetMoveRespondType.Timeout);
this.motorCmdType = MotorCmdType.None; hasNextMotorMultiTargetCmd = false; return;
}
// ---- toio ID missed Error ----
if (!this.onMat)
{
this.multiTargetMoveCallback?.Invoke(cmd.configID, Cube.TargetMoveRespondType.ToioIDmissed);
this.motorCmdType = MotorCmdType.None; hasNextMotorMultiTargetCmd = false; return;
}
// ---- Parameter Error ----
if (cmd.xs[cmd.idx] == 65535 && cmd.ys[cmd.idx] == 65535 &&
(cmd.rotTypes[cmd.idx] == Cube.TargetRotationType.Original || cmd.rotTypes[cmd.idx] == Cube.TargetRotationType.NotRotate))
{
this.multiTargetMoveCallback?.Invoke(cmd.configID, Cube.TargetMoveRespondType.ParameterError);
this.motorCmdType = MotorCmdType.None; hasNextMotorMultiTargetCmd = false; return;
}
// ---- Preprocess ----
if (cmd.xs[cmd.idx] == 65535 && cmd.idx > 0)
{
this.motorMultiTargetCmd.xs[cmd.idx] = cmd.xs[cmd.idx] = cmd.xs[cmd.idx - 1];
}
if (cmd.ys[cmd.idx] == 65535 && cmd.idx > 0)
{
this.motorMultiTargetCmd.ys[cmd.idx] = cmd.ys[cmd.idx] = cmd.ys[cmd.idx - 1];
}
Vector2 targetPos = new Vector2(cmd.xs[cmd.idx], cmd.ys[cmd.idx]);
Vector2 pos = new Vector2(this.x, this.y);
var dpos = targetPos - pos;
// ---- Reach ----
// reach pos
if (!cmd.reach && dpos.magnitude < motorTargetPosTol)
{
this.motorMultiTargetCmd.reach = true;
var rotType = cmd.rotTypes[cmd.idx];
if (rotType == Cube.TargetRotationType.NotRotate) // Not rotate
{
this.motorMultiTargetCmd.absoluteDeg = Deg(this.deg);
}
else if (rotType == Cube.TargetRotationType.Original) // Inital deg
{
this.motorMultiTargetCmd.absoluteDeg = Deg(cmd.initialDeg);
}
else if ((byte)rotType <= 2) // Absolute deg
{
this.motorMultiTargetCmd.absoluteDeg = Deg(cmd.degs[cmd.idx]);
}
else // Relative deg
{
this.motorMultiTargetCmd.absoluteDeg = Deg(this.deg + cmd.degs[cmd.idx]);
this.motorMultiTargetCmd.relativeDeg = cmd.degs[cmd.idx];
this.motorMultiTargetCmd.lastDeg = this.deg;
}
}
// reach deg
if (cmd.reach && Mathf.Abs(Deg(this.deg - cmd.absoluteDeg)) < 15 && cmd.relativeDeg < 180)
{
// This is the last target
if (cmd.idx == cmd.xs.Length - 1)
{
this.targetMoveCallback?.Invoke(cmd.configID, Cube.TargetMoveRespondType.Normal);
// Load Next Command
if (hasNextMotorMultiTargetCmd)
{
motorMultiTargetCmd = nextMotorMultiTargetCmd;
hasNextMotorMultiTargetCmd = false;
MultiTargetMoveInit();
}
// Over
else
{
this.motorCmdType = MotorCmdType.None; hasNextMotorMultiTargetCmd = false; return;
}
}
// Not the last target
else
{
MultiTargetMove_NextIdx();
}
}
// ---- Update ----
cmd = motorMultiTargetCmd;
// ---- Rotate ----
if (cmd.reach)
{
float relativeDeg;
(translate, rotate, relativeDeg) = TargetMove_RotateControl(
cmd.absoluteDeg, cmd.relativeDeg, cmd.lastDeg, cmd.rotTypes[cmd.idx], cmd.maxSpd);
if (cmd.rotTypes[cmd.idx] == Cube.TargetRotationType.RelativeClockwise ||
cmd.rotTypes[cmd.idx] == Cube.TargetRotationType.RelativeCounterClockwise)
{
this.motorMultiTargetCmd.relativeDeg = relativeDeg;
}
}
// ---- Move ----
else
{
(translate, rotate) = TargetMove_MoveControl(elipsed,
cmd.xs[cmd.idx], cmd.ys[cmd.idx], cmd.maxSpd, cmd.targetSpeedType, cmd.acc, cmd.targetMoveType);
}
// ---- Apply ----
ApplyMotorControl(translate, rotate);
}
// COMMAND API
public override void MultiTargetMove(
int[] targetXList,
int[] targetYList,
int[] targetAngleList,
Cube.TargetRotationType[] multiRotationTypeList,
int configID,
int timeOut,
Cube.TargetMoveType targetMoveType,
int maxSpd,
Cube.TargetSpeedType targetSpeedType,
Cube.MultiWriteType multiWriteType
)
{
#if !RELEASE
if (targetXList.Length == 0 || targetYList.Length == 0 || targetAngleList.Length == 0 ||
!(targetXList.Length == targetYList.Length && targetYList.Length == targetAngleList.Length))
{
Debug.LogErrorFormat("[Cube.TargetMove]座標・角度リストのサイズが不一致または０. targetXList.Length={0}, targetYList.Length={1}, targetAngleList.Length={2}", targetXList.Length, targetYList.Length, targetAngleList.Length);
}
if (255 < configID)
{
Debug.LogErrorFormat("[Cube.TargetMove]制御識別値範囲を超えました. configID={0}", configID);
}
if (255 < timeOut)
{
Debug.LogErrorFormat("[Cube.TargetMove]制御時間範囲を超えました. timeOut={0}", timeOut);
}
if (this.maxMotor < maxSpd)
{
Debug.LogErrorFormat("[Cube.TargetMove]速度範囲を超えました. maxSpd={0}", maxSpd);
}
#endif
MotorMultiTargetCmd cmd = new MotorMultiTargetCmd();
cmd.xs = Array.ConvertAll(targetXList, new Converter <int, ushort>(x => (ushort)(x == -1?65535:Mathf.Clamp(x, 0, 65534))));
if (cmd.xs.Length == 0)
{
return;
}
cmd.xs = cmd.xs.Take(29).ToArray();
cmd.ys = Array.ConvertAll(targetYList, new Converter <int, ushort>(x => (ushort)(x == -1?65535:Mathf.Clamp(x, 0, 65534))));
if (cmd.ys.Length == 0)
{
return;
}
cmd.ys = cmd.ys.Take(29).ToArray();
cmd.degs = Array.ConvertAll(targetAngleList, new Converter <int, ushort>(x => (ushort)x));
if (cmd.degs.Length == 0)
{
return;
}
cmd.degs = cmd.degs.Take(29).ToArray();
cmd.rotTypes = multiRotationTypeList != null? multiRotationTypeList : new Cube.TargetRotationType[cmd.xs.Length];
cmd.configID = (byte)Mathf.Clamp(configID, 0, 255);
cmd.timeOut = (byte)Mathf.Clamp(timeOut, 0, 255);
cmd.maxSpd = (byte)Mathf.Clamp(maxSpd, 0, this.maxMotor);
cmd.targetMoveType = targetMoveType;
cmd.targetSpeedType = targetSpeedType;
cmd.multiWriteType = multiWriteType;
cmd.tRecv = Time.time;
MotorSetCmd(MotorCmdType.MotorMultiTargetCmd, cmd);
}
protected override void MotorScheduler(float t)
{
float latestRecvTime = 0;
bool newCmd = false;
string oldCmdType = motorCurrentCmdType;
MotorAccCmd oldAccCmd = currMotorAccCmd;
MotorMultiTargetCmd multiCmdTemp = default;
byte oldConfigID = 0;
switch (motorCurrentCmdType)
{
case "Target": { oldConfigID = currMotorTargetCmd.configID; break; }
case "MultiTarget": { oldConfigID = currMotorMultiTargetCmd.configID; break; }
}
bool overwriteMulti = false;
// ----- Dequeue Commands -----
while (motorTimeCmdQ.Count > 0 && t > motorTimeCmdQ.Peek().tRecv)
{
currMotorTimeCmd = motorTimeCmdQ.Dequeue();
motorCurrentCmdType = "Time"; latestRecvTime = currMotorTimeCmd.tRecv;
newCmd = true;
overwriteMulti = true;
}
while (motorTargetCmdQ.Count > 0 && t > motorTargetCmdQ.Peek().tRecv)
{
currMotorTargetCmd = motorTargetCmdQ.Dequeue();
if (currMotorTargetCmd.tRecv > latestRecvTime)
{
motorCurrentCmdType = "Target"; latestRecvTime = currMotorTargetCmd.tRecv;
newCmd = true;
}
overwriteMulti = true;
}
while (motorMultiTargetCmdQ.Count > 0 && t > motorMultiTargetCmdQ.Peek().tRecv)
{
multiCmdTemp = motorMultiTargetCmdQ.Dequeue();
if (multiCmdTemp.tRecv > latestRecvTime)
{
motorCurrentCmdType = "MultiTarget"; latestRecvTime = multiCmdTemp.tRecv;
newCmd = true;
}
}
while (motorAccCmdQ.Count > 0 && t > motorAccCmdQ.Peek().tRecv)
{
currMotorAccCmd = motorAccCmdQ.Dequeue();
if (currMotorAccCmd.tRecv > latestRecvTime)
{
motorCurrentCmdType = "Acc"; latestRecvTime = currMotorAccCmd.tRecv;
newCmd = true;
}
overwriteMulti = true;
}
// ----- elipsed -----
float elipsed = 0;
switch (motorCurrentCmdType)
{
case "Time": elipsed = t - currMotorTimeCmd.tRecv; break;
case "Target": elipsed = t - currMotorTargetCmd.tRecv; break;
case "MultiTarget": elipsed = t - multiCmdTemp.tRecv; break;
case "Acc": elipsed = t - currMotorAccCmd.tRecv; break;
}
// ----- Target -----
if (newCmd && oldCmdType == "Target")
{
this.targetMoveCallback?.Invoke(oldConfigID, Cube.TargetMoveRespondType.OtherWrite);
}
if (newCmd && motorCurrentCmdType == "Target")
{
TargetMoveInit();
}
// ----- Multi Target -----
if (overwriteMulti)
{
hasNextMotorMultiTargetCmd = false;
}
if (oldCmdType == "MultiTarget" && overwriteMulti)
{
this.multiTargetMoveCallback?.Invoke(oldConfigID, Cube.TargetMoveRespondType.OtherWrite);
if (motorCurrentCmdType == "MultiTarget" && newCmd)
{
currMotorMultiTargetCmd = multiCmdTemp;
MultiTargetMoveInit();
}
}
else if (oldCmdType != "MultiTarget")
{
if (motorCurrentCmdType == "MultiTarget" && newCmd)
{
currMotorMultiTargetCmd = multiCmdTemp;
MultiTargetMoveInit();
}
}
else if (newCmd) // oldCmdType == "MultiTarget" && !overwriteMulti
{
if (multiCmdTemp.multiWriteType == Cube.MultiWriteType.Write)
{
this.multiTargetMoveCallback?.Invoke(oldConfigID, Cube.TargetMoveRespondType.OtherWrite);
currMotorMultiTargetCmd = multiCmdTemp;
MultiTargetMoveInit();
}
else if (this.hasNextMotorMultiTargetCmd)
{
this.multiTargetMoveCallback?.Invoke(multiCmdTemp.configID, Cube.TargetMoveRespondType.AddRefused);
}
else
{
this.nextMotorMultiTargetCmd = multiCmdTemp;
this.hasNextMotorMultiTargetCmd = true;
}
}
// ----- Acceleration -----
if (newCmd && motorCurrentCmdType == "Acc")
{
if (oldCmdType == "Acc") // 前指令がAccの場合
{
currMotorAccCmd.initialSpd = oldAccCmd.currSpd; // 速度を継続する
}
else // 前指令がAccじゃない場合
{
currMotorAccCmd.initialSpd = 0; // 速度を継続する ※リアルのテスト結果
}
// currMotorAccCmd.acc = 0; // 直ちに目標速度にする ※仕様書
}
// ----- Excute Order -----
switch (motorCurrentCmdType)
{
case "Time":
{
if (currMotorTimeCmd.duration == 0 ||
elipsed < currMotorTimeCmd.duration / 1000f)
{
motorCmdL = currMotorTimeCmd.left;
motorCmdR = currMotorTimeCmd.right;
}
else
{
motorCmdL = 0; motorCmdR = 0;
}
break;
}
case "Target":
{
TargetMoveController(elipsed);
break;
}
case "MultiTarget":
{
MultiTargetMoveController(elipsed);
break;
}
case "Acc":
{
AccMoveController(elipsed);
break;
}
}
}
