// COMMAND API
public override void AccelerationMove(
int targetSpeed,
int acceleration,
int rotationSpeed,
Cube.AccPriorityType accPriorityType,
int controlTime
)
{
#if !RELEASE
if (this.maxMotor < Mathf.Abs(targetSpeed))
{
Debug.LogErrorFormat("[Cube.AccelerationMove]直線速度範囲を超えました. targetSpeed={0}", targetSpeed);
}
if (255 < acceleration)
{
Debug.LogErrorFormat("[Cube.AccelerationMove]加速度範囲を超えました. acceleration={0}", acceleration);
}
if (65535 < rotationSpeed)
{
Debug.LogErrorFormat("[Cube.AccelerationMove]回転速度範囲を超えました. rotationSpeed={0}", rotationSpeed);
}
if (255 < controlTime)
{
Debug.LogErrorFormat("[Cube.AccelerationMove]制御時間範囲を超えました. controlTime={0}", controlTime);
}
#endif
MotorAccCmd cmd = new MotorAccCmd();
cmd.spd = Mathf.Clamp(targetSpeed, -this.maxMotor, this.maxMotor);
cmd.acc = (byte)Mathf.Clamp(acceleration, 0, 255);
cmd.rotationSpeed = Mathf.Clamp(rotationSpeed, -65535, 65535);
cmd.accPriorityType = accPriorityType;
cmd.controlTime = (byte)Mathf.Clamp(controlTime, 0, 255);
cmd.tRecv = Time.time;
motorAccCmdQ.Enqueue(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;
}
}
}
