public static UniTask <Cube.TargetMoveRespondType> TargetMove(Cube cube,
int targetX, int targetY, int targetAngle, int configID = 0, int timeOut = 0,
Cube.TargetMoveType targetMoveType = Cube.TargetMoveType.RotatingMove, int maxSpd = 80,
Cube.TargetSpeedType targetSpeedType = Cube.TargetSpeedType.UniformSpeed,
Cube.TargetRotationType targetRotationType = Cube.TargetRotationType.AbsoluteLeastAngle,
Cube.ORDER_TYPE order = Cube.ORDER_TYPE.Strong)
{
var source = new UniTaskCompletionSource <Cube.TargetMoveRespondType>();
var callbackKey = Guid.NewGuid().ToString();
cube.targetMoveCallback.RemoveListener(callbackKey);
cube.targetMoveCallback.AddListener(callbackKey, (_cube, _configId, _respondType) =>
{
if (_configId == configID)
{
DelayedRemoveTargetMoveCallback(cube, callbackKey);
source.TrySetResult(_respondType);
}
});
cube.TargetMove(targetX, targetY, targetAngle, configID, timeOut,
targetMoveType, maxSpd, targetSpeedType, targetRotationType, order);
return(source.Task);
}
public bool idle; // Movement with idle=true won't be excuted by move()
public Movement(CubeHandle handle, double translate, double rotate,
int durationMs = 500, bool reached = false, bool idle = false, Cube.ORDER_TYPE order = Cube.ORDER_TYPE.Weak)
{
this.handle = handle;
this.translate = translate; this.rotate = rotate;
this.durationMs = durationMs; this.reached = reached; this.idle = idle;
this.order = order;
}
// https://toio.github.io/toio-spec/docs/ble_motor#%E3%83%A2%E3%83%BC%E3%82%BF%E3%83%BC%E5%88%B6%E5%BE%A1
public static void MoveWithRadPerSec(this Cube self, float left_radPerSec, float right_radPerSec,
int durationMs, Cube.ORDER_TYPE order = Cube.ORDER_TYPE.Weak)
{
Func <float, float> radPerSec2rpm = (float radPerSec) => { return(Mathf.Sign(radPerSec) * Mathf.Clamp(Mathf.Abs(radPerSec * 30.0f / Mathf.PI), 34.0f, 494.0f)); };
Func <float, int> rpm2inputval = (float rpm) => { return((int)(rpm * (107.0f / 460.0f) + (42.0f / 460.0f))); };
self.Move(rpm2inputval(radPerSec2rpm(left_radPerSec)),
rpm2inputval(radPerSec2rpm(right_radPerSec)),
durationMs, order);
}
/// <summary>
/// Move with Movement and explicitly given duration and order type.
/// Return actually sent Movement
/// </summary>
public Movement Move(Movement mv, int durationMs, Cube.ORDER_TYPE order, bool border = true)
{
if (mv.idle)
{
return(mv);
}
var mv_ = Move(mv.translate, mv.rotate, durationMs, border, order);
mv_.reached = mv.reached;
return(mv_);
}
public SendData(Cube _instance, Action _func, Cube.ORDER_TYPE _type)
{
this.instance = _instance;
this.func = _func;
this.type = _type;
this.frameStamp = Time.frameCount;
#if !RELEASE
this.DEBUG_name = "";
this.DEBUG_param = null;
#endif
}
/// <summary>
/// 命令キューに命令を追加
/// </summary>
public void AddOrder(Cube instance, Action func, Cube.ORDER_TYPE _type)
{
if (!this.cubeTable.ContainsKey(instance))
{
this.cubeTable.Add(instance, new CubeData());
}
var cube = this.cubeTable[instance];
if (this.IsStrong(_type))
{
cube.strong_cnt++;
}
cube.sendQueue.Enqueue(new SendData(instance, func, _type));
}
public void DEBUG_AddOrder(Cube instance, Action func, Cube.ORDER_TYPE _type, string DEBUG_name, object[] DEBUG_plist)
{
if (!this.cubeTable.ContainsKey(instance))
{
this.cubeTable.Add(instance, new CubeData());
}
var cube = this.cubeTable[instance];
if (this.IsStrong(_type))
{
cube.strong_cnt++;
}
var data = new SendData(instance, func, _type);
data.DEBUG_name = DEBUG_name;
data.DEBUG_param = DEBUG_plist;
cube.sendQueue.Enqueue(data);
}
//////////////////////////////
// Command - Basic
// Each call sends 1 order to Cube
//////////////////////////////
/// <summary>
/// Move with order of left/right motors.
/// Orders are saved for prediction, so use this instead of Cube.move.
/// </summary>
public void MoveRaw(double uL, double uR, int durationMs = 1000, Cube.ORDER_TYPE order = Cube.ORDER_TYPE.Weak)
{
uL = clip(uL, -MaxSpd, MaxSpd);
uR = clip(uR, -MaxSpd, MaxSpd);
if (durationMs < 10)
{
if (durationMs <= 5)
{
uL = 0; uR = 0;
}
durationMs = 10;
}
cube.Move(ToInt(uL), ToInt(uR), durationMs, order);
// Save orders for prediction
var now = Time.time;
int size = (int)(lag / dt + 4);
uLHist.Add(uL); uRHist.Add(uR); sentTimeHist.Add(now); durationHist.Add(durationMs / 1000f);
while (uLHist.Count > size)
{
uLHist.RemoveAt(0);
}
while (uRHist.Count > size)
{
uRHist.RemoveAt(0);
}
while (sentTimeHist.Count > size)
{
sentTimeHist.RemoveAt(0);
}
while (durationHist.Count > size)
{
durationHist.RemoveAt(0);
}
}
protected void Request(string characteristicName, byte[] buff, bool withResponse, Cube.ORDER_TYPE order, string DEBUG_name, params object[] DEBUG_plist)
{
if (!isConnected)
{
return;
}
#if RELEASE
CubeOrderBalancer.Instance.AddOrder(this, () => this.characteristicTable[characteristicName].WriteValue(buff, withResponse), order);
#else
CubeOrderBalancer.Instance.DEBUG_AddOrder(this, () => this.characteristicTable[characteristicName].WriteValue(buff, withResponse), order, DEBUG_name, DEBUG_plist);
#endif
}
/// <summary>
/// Move with translation ordr, rotation order, duration.
/// 前進指令、回転指令、継続時間で移動
/// Return actually sent Movement
/// </summary>
public Movement Move(double translate, double rotate, int durationMs = 1000,
bool border = true, Cube.ORDER_TYPE order = Cube.ORDER_TYPE.Weak)
{
// transform order. 指令形式変換
double uL = translate + rotate / 2;
double uR = translate - rotate / 2;
// --- Deadzone processing ---
// Simply add offset to avoid deadzone.
// 単純に0じゃない指令値をDeadzone外に引っ張り出す
// {
// if (uL > 0) uL += Deadzone;
// if (uL < 0) uL -= Deadzone;
// if (uR > 0) uR += Deadzone;
// if (uR < 0) uR -= Deadzone;
// }
// Adjust uneffective uL,uR to nearest effective uL,uR
// Deadzone外の有効値に一番近い指令にする、回転指令の維持を優先的に考慮
{
var l = uL; var r = uR; var M = Max(l, r); var m = Min(l, r);
if ((l == 0 || l >= Deadzone || l <= -Deadzone) && (r == 0 || r >= Deadzone || r <= -Deadzone))
{
// Outside deadzone
}
else if (Abs(l - r) < Deadzone)
{
if ((l + r) > 0)
{
uL += Deadzone - m; uR += Deadzone - m;
}
else if ((l + r) < 0)
{
uL += -Deadzone - M; uR += -Deadzone - M;
}
else
{
} //uL=Sign(uL)*Deadzone; uR=Sign(uR)*Deadzone;}
}
else if (Abs(l - r) < 2 * Deadzone)
{
if ((l + r) > 0)
{
if (m < 0)
{
uL += 0 - m; uR += 0 - m;
}
else if (m < Deadzone / 2)
{
uL += 0 - m; uR += 0 - m;
}
else
{
uL += Deadzone - m; uR += Deadzone - m;
}
}
else if ((l + r) < 0)
{
if (M > 0)
{
uL += 0 - M; uR += 0 - M;
}
else if (M > -Deadzone / 2)
{
uL += 0 - M; uR += 0 - M;
}
else
{
uL += -Deadzone - M; uR += -Deadzone - M;
}
}
else
{
uL = Sign(l) * Deadzone; uR = Sign(r) * Deadzone;
}
}
else
{
if ((l + r) > 0)
{
if (m < -Deadzone / 2)
{
uL += -Deadzone - m; uR += -Deadzone - m;
}
else if (m > Deadzone / 2)
{
uL += Deadzone - m; uR += Deadzone - m;
}
else
{
uL += 0 - m; uR += 0 - m;
}
}
else
{
if (m < -Deadzone / 2)
{
uL += -Deadzone - M; uR += -Deadzone - M;
}
else if (m > Deadzone / 2)
{
uL += Deadzone - M; uR += Deadzone - M;
}
else
{
uL += 0 - M; uR += 0 - M;
}
}
}
}
// truncate
if (Max(uL, uR) > MaxSpd)
{
uL -= Max(uL, uR) - MaxSpd;
uR -= Max(uL, uR) - MaxSpd;
}
else if (Min(uL, uR) < -MaxSpd)
{
uL -= Min(uL, uR) + MaxSpd;
uR -= Min(uL, uR) + MaxSpd;
}
// transform order
translate = (uL + uR) / 2;
rotate = uL - uR;
// --- Border Limitation ---
// Predict trajectory and cut it before crossing border, by cutting duration.
// ボーダー制限：ボーダーから出ないようdurationを制限する
int dur = durationMs;
if (border)
{
// parameters
double rx = (double)RangeX / 2; double ry = (double)RangeY / 2; double e = 0.4;
// predicted final stopping state, assuming not output current order.
double x = this.stopXPred, y = this.stopYPred, rad = this.radPred;
// predicted state if not stopping.
double predX = x + Max(spdPred, translate * VDotOverU) * dt * Cos(rad);
double predY = y + Max(spdPred, translate * VDotOverU) * dt * Sin(rad);
double predRad = this.radPred;
// currently outside and going further : stop transition
if ((Abs(x - CenterX) >= rx || Abs(y - CenterY) >= ry) &&
(Abs(predX - CenterX) >= rx && Abs(predX - CenterX) >= Abs(x - CenterX) ||
Abs(predY - CenterY) >= ry && Abs(predY - CenterY) >= Abs(y - CenterY)))
{
// stop
translate = 0;
// Help rotate back to insider
if (Abs(rotate) < 2 * Deadzone &&
(x - CenterX > rx && y - CenterY > ry && (PI - e < rad && rad < PI || PI / 2 - e < -rad && -rad < PI / 2) ||
x - CenterX > rx && y - CenterY < -ry && (PI - e < -rad && -rad < PI || PI / 2 - e < rad && rad < PI / 2) ||
x - CenterX < -rx && y - CenterY < -ry && (0 < -rad && -rad < 0 + e || PI / 2 < rad && rad < PI / 2 + e) ||
x - CenterX < -rx && y - CenterY > ry && (0 < rad && rad < 0 + e || PI / 2 < -rad && -rad < PI / 2 + e) ||
x - CenterX > rx && Abs(y - CenterY) <= ry && (PI / 2 - e < rad && rad < PI / 2 || PI / 2 - e < -rad && -rad < PI / 2) ||
x - CenterX < -rx && Abs(y - CenterY) <= ry && (PI / 2 < rad && rad < PI / 2 + e || PI / 2 < -rad && -rad < PI / 2 + e) ||
y - CenterY > ry && Abs(x - CenterX) <= rx && (0 < rad && rad < e || PI - e < rad && rad < PI) ||
y - CenterY < -ry && Abs(x - CenterX) <= rx && (0 < -rad && -rad < e || PI - e < -rad && -rad < PI)
)
)
{
rotate = 2 * Deadzone * Sign(rotate);
}
}
// currently inside : limit duration
else if (Abs(x - CenterX) < rx && Abs(y - CenterY) < ry)
{
var _dt = 0.05f;
var now = Time.time;
double spdL = uL * VDotOverU;
double spdR = uR * VDotOverU;
predX = x; predY = y;
for (double t = 0; t < durationMs / 1000f; t += _dt)
{
predRad += (float)((spdL - spdR) / TireWidthDot) * _dt;
predRad = Rad(predRad);
predX += Cos(predRad) * (spdL + spdR) / 2 * _dt;
predY += Sin(predRad) * (spdL + spdR) / 2 * _dt;
if (Abs(predX - CenterX) >= rx || Abs(predY - CenterY) >= ry)
{
dur = (int)(t * 1000 - _dt * 1000);
if (dur < 10)
{
dur = 0;
}
break;
}
}
}
}
// transform
uL = translate + rotate / 2;
uR = translate - rotate / 2;
MoveRaw(uL, uR, dur, order);
return(new Movement(this, translate, rotate, dur, false, false));
}
private bool IsStrong(Cube.ORDER_TYPE order)
{
return(Cube.ORDER_TYPE.Strong == order);
}
public static void TurnLedOn(Cube cube, Color color, int durationMs, Cube.ORDER_TYPE order = Cube.ORDER_TYPE.Strong)
{
cube.TurnLedOn(ColorByteValue(color.r), ColorByteValue(color.g), ColorByteValue(color.b), durationMs, order);
}
