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]座標・角度リストのサイズが不一致または0. 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; } } }