/// <summary>
/// 緊急時ハンドルコントロール
/// </summary>
/// <param name="lrfData">270度＝個数のデータを想定</param>
/// <returns></returns>
public double CheckEHS(double[] lrfData)
{
double rtHandleVal = 0.0;
Result = EHS_MODE.None;
// 指定の扇状角度の指定の距離範囲内に、障害物があればハンドルを切る。
{
double lrfScale = URG_LRF.getScale(); // スケール変換値
const int rangeCenterAng = 270 / 2; // 中央(角度)
double LHitLength = 0.0;
double LHitDir = 0.0;
double RHitLength = 0.0;
double RHitDir = 0.0;
double minScaledRange = (MinRange * lrfScale);
double maxScaleRange = (MaxRange * lrfScale);
double minDistance;
// LRFの値を調べる
// 右側
minDistance = maxScaleRange;
for (int i = (stRAng + rangeCenterAng); i < (edRAng + rangeCenterAng); i++)
{
// 範囲内なら反応
if (lrfData[i] > minScaledRange && lrfData[i] < maxScaleRange)
{
// もっとも近い障害物を選定
if (lrfData[i] < minDistance)
{
minDistance = lrfData[i];
RHitLength = lrfData[i];
RHitDir = (double)i;
}
}
}
// 左側
minDistance = maxScaleRange;
for (int i = (edLAng + rangeCenterAng); i >= (stLAng + rangeCenterAng); i--)
{
// 範囲内なら反応
if (lrfData[i] > minScaledRange && lrfData[i] < maxScaleRange)
{
if (lrfData[i] < minDistance)
{
minDistance = lrfData[i];
LHitLength = lrfData[i];
LHitDir = (double)i;
}
}
}
// ハンドル角を計算
#if false
if (LHitLength != 0.0 && RHitLength != 0.0)
{
// 左右がHIT
double Lx, Ly;
double Rx, Ry;
Lx = Ly = 0.0;
LrfValToPosition(ref Lx, ref Ly, LHitDir, LHitLength);
Rx = Ry = 0.0;
LrfValToPosition(ref Rx, ref Ry, RHitDir, RHitLength);
double tgtAng = Rooting.CalcPositionToAngle((Lx - Rx) * 0.5, (Ly - Ry) * 0.5);
rtHandleVal = Rooting.CalcHandleValueFromAng(tgtAng, 0.0);
Result = EHS_MODE.CenterPass;
}
else if (LHitLength != 0.0)
#else
if (LHitLength != 0.0 && RHitLength != 0.0)
{
// 両方ならば、近いほうを優先する
if (LHitLength < RHitLength)
{
RHitLength = 0.0;
}
else
{
LHitLength = 0.0;
}
}
if (LHitLength != 0.0)
#endif
{
// 左側がHit
double Lx, Ly;
Lx = Ly = 0.0;
LrfValToPosition(ref Lx, ref Ly, LHitDir, LHitLength);
// 壁から離れる距離を足しこむ
Lx += WallRange;
double tgtAng = 45.0;// Rooting.CalcPositionToAngle(Lx, Ly);
rtHandleVal = Rooting.CalcHandleValueFromAng(tgtAng, 0.0);
Result = EHS_MODE.LeftWallHit;
}
else if (RHitLength != 0.0)
{
// 右側がHit
double Rx, Ry;
Rx = Ry = 0.0;
LrfValToPosition(ref Rx, ref Ry, RHitDir, RHitLength);
Rx -= WallRange;
double tgtAng = -45.0;//Rooting.CalcPositionToAngle(Rx, Ry);
rtHandleVal = Rooting.CalcHandleValueFromAng(tgtAng, 0.0);
Result = EHS_MODE.RightWallHit;
}
}
// 極小値は切り捨て
rtHandleVal = (double)((int)(rtHandleVal * 100.0)) / 100.0;
return(rtHandleVal);
}
/// <summary>
/// 緊急時ハンドルコントロール
/// </summary>
/// <param name="lrfData">270度＝個数のデータを想定</param>
/// <returns></returns>
public double CheckEHS(double[] lrfData)
{
double rtHandleVal = 0.0;
Result = EHS_MODE.None;
// 指定の扇状角度の指定の距離範囲内に、障害物があればハンドルを切る。
{
double lrfScale = URG_LRF.getScale(); // スケール変換値
const int rangeCenterAng = 270 / 2; // 中央(角度)
double LHitLength = 0.0;
double LHitDir = 0.0;
double RHitLength = 0.0;
double RHitDir = 0.0;
double minScaledRange = (MinRange * lrfScale);
double maxScaleRange = (MaxRange * lrfScale);
double minDistance;
// LRFの値を調べる
// 右側
minDistance = maxScaleRange;
for (int i = (stRAng + rangeCenterAng); i < (edRAng + rangeCenterAng); i++)
{
// 範囲内なら反応
if (lrfData[i] > minScaledRange && lrfData[i] < maxScaleRange)
{
// もっとも近い障害物を選定
if (lrfData[i] < minDistance)
{
minDistance = lrfData[i];
RHitLength = lrfData[i];
RHitDir = (double)i;
}
}
}
// 左側
minDistance = maxScaleRange;
for (int i = (edLAng + rangeCenterAng); i >= (stLAng + rangeCenterAng); i--)
{
// 範囲内なら反応
if (lrfData[i] > minScaledRange && lrfData[i] < maxScaleRange)
{
if (lrfData[i] < minDistance)
{
minDistance = lrfData[i];
LHitLength = lrfData[i];
LHitDir = (double)i;
}
}
}
// ハンドル角を計算
#if false
if (LHitLength != 0.0 && RHitLength != 0.0)
{
// 左右がHIT
double Lx, Ly;
double Rx, Ry;
Lx = Ly = 0.0;
LrfValToPosition(ref Lx, ref Ly, LHitDir, LHitLength);
Rx = Ry = 0.0;
LrfValToPosition(ref Rx, ref Ry, RHitDir, RHitLength);
double tgtAng = Rooting.CalcPositionToAngle((Lx - Rx) * 0.5, (Ly - Ry) * 0.5);
rtHandleVal = Rooting.CalcHandleValueFromAng(tgtAng, 0.0);
Result = EHS_MODE.CenterPass;
}
else if (LHitLength != 0.0)
#else
if (LHitLength != 0.0 && RHitLength != 0.0)
{
// 両方ならば、近いほうを優先する
if (LHitLength < RHitLength) RHitLength = 0.0;
else LHitLength = 0.0;
}
if (LHitLength != 0.0)
#endif
{
// 左側がHit
double Lx, Ly;
Lx = Ly = 0.0;
LrfValToPosition(ref Lx, ref Ly, LHitDir, LHitLength);
// 壁から離れる距離を足しこむ
Lx += WallRange;
double tgtAng = 45.0;// Rooting.CalcPositionToAngle(Lx, Ly);
rtHandleVal = Rooting.CalcHandleValueFromAng(tgtAng, 0.0);
Result = EHS_MODE.LeftWallHit;
}
else if (RHitLength != 0.0)
{
// 右側がHit
double Rx, Ry;
Rx = Ry = 0.0;
LrfValToPosition(ref Rx, ref Ry, RHitDir, RHitLength);
Rx -= WallRange;
double tgtAng = -45.0;//Rooting.CalcPositionToAngle(Rx, Ry);
rtHandleVal = Rooting.CalcHandleValueFromAng(tgtAng, 0.0);
Result = EHS_MODE.RightWallHit;
}
}
// 極小値は切り捨て
rtHandleVal = (double)((int)(rtHandleVal * 100.0)) / 100.0;
// ※LRを入れ替えて、このマイナスをはずせば意味があう? 2016.11.05
return -rtHandleVal;
}
