protected override void ResetMotor()
{
base.ResetMotor();
motorCmdType = MotorCmdType.None;
motorTimeCmd = default;
}
protected override void ResetMotor()
{
base.ResetMotor();
motorTimeCmdQ.Clear();
currMotorTimeCmd = default;
}
// ============ Commands ============
public override void Move(int left, int right, int durationMS)
{
MotorTimeCmd cmd = new MotorTimeCmd();
cmd.left = Mathf.Clamp(left, -maxMotor, maxMotor);
cmd.right = Mathf.Clamp(right, -maxMotor, maxMotor);
if (Mathf.Abs(cmd.left) < this.deadzone)
{
cmd.left = 0;
}
if (Mathf.Abs(cmd.right) < this.deadzone)
{
cmd.right = 0;
}
cmd.duration = Mathf.Clamp(durationMS, 0, 2550);
cmd.tRecv = Time.time;
motorTimeCmdQ.Enqueue(cmd);
}
protected MotorTimeCmd currMotorTimeCmd = default; // current command
protected virtual void MotorScheduler(float t)
{
while (motorTimeCmdQ.Count > 0 && t > motorTimeCmdQ.Peek().tRecv)
{
currMotorTimeCmd = motorTimeCmdQ.Dequeue();
}
var elipsed = t - currMotorTimeCmd.tRecv;
// ----- Excute Order -----
if (currMotorTimeCmd.duration == 0 ||
elipsed < currMotorTimeCmd.duration / 1000f)
{
motorCmdL = currMotorTimeCmd.left;
motorCmdR = currMotorTimeCmd.right;
}
else
{
motorCmdL = 0; motorCmdR = 0;
}
}