public void Reset(string mapFileName)
{
ModeCtrl = new ModeControl();
//
MapFile = MapData.LoadMapFile(mapFileName);
//
RTS = new Rooting(MapFile);
// マップ画像ファイル名、実サイズの横[mm], 実サイズ縦[mm] (北向き基準)
LocSys = new LocPreSumpSystem(MapFile.MapImageFileName, MapFile.RealWidth, MapFile.RealHeight);
UpdateCnt = 0;
goalFlg = false;
handleValue = 0.0;
accValue = 0.0;
Reset_StartPosition();
}
/// <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>
/// 定期更新(100ms周期)
/// </summary>
/// <param name="LocSys"></param>
/// <param name="useEBS">緊急ブレーキ</param>
/// <param name="useEHS">壁避け動作</param>
/// <param name="bStraightMode">直進モード</param>
/// <returns>true...バック中(緊急動作しない)</returns>
///
public bool Update(LocPreSumpSystem LocSys, bool useEBS, bool useEHS)
{
// LRFデータ取得
double[] lrfData = LocSys.LRF.getData();
// Rooting ------------------------------------------------------------------------------------------------
ModeCtrl.update();
if (ModeCtrl.GetActionMode() == ModeControl.ActionMode.CheckPoint)
{
// ルート進行
// ルート算定
// 現在座標更新
RTS.setNowPostion((int)LocSys.GetResultLocationX(),
(int)LocSys.GetResultLocationY(),
(int)LocSys.GetResultAngle());
// ルート計算
RTS.calcRooting();
// チェックポイント通過をLEDで伝える
if (RTS.IsCheckPointPass())
{
CarCtrl.SetHeadMarkLED((int)CersioCtrl.LED_PATTERN.WHITE_FLASH, true);
}
}
else if (ModeCtrl.GetActionMode() == ModeControl.ActionMode.Avoid)
{
// 回避ルート進行
// ルート算定
// 現在座標更新
avoidRTS.setNowPostion((int)LocSys.GetResultLocationX(),
(int)LocSys.GetResultLocationY(),
(int)LocSys.GetResultAngle());
// ルート計算
avoidRTS.calcRooting();
// 回避中をLEDで伝える
//CarCtrl.SetHeadMarkLED((int)CersioCtrl.LED_PATTERN.WHITE_FLASH, false);
}
// DriveControl -------------------------------------------------------------------------------------------
// マップ情報取得
{
Grid nowGrid = LocSys.GetMapInfo(LocSys.R1);
// マップ情報反映
if (nowGrid == Grid.BlueArea)
{
// スローダウン
EBS.AccelSlowDownCnt = 5;
Brain.addLogMsg += "ColorMap:Blue\n";
}
// 壁の中にいる
if (nowGrid == Grid.RedArea )
{
// 強制補正エリア
Brain.addLogMsg += "LocRivision:ColorMap[Red](マップ情報の位置補正)\n";
bRevisionRequest = true;
}
// 補正実行エリア
{
if (nowGrid == Grid.GreenArea && !bGreenAreaFlg)
{
// 補正指示のエリアに入った
Brain.addLogMsg += "LocRivision:ColorMap[Green](マップ情報の位置補正)\n";
bGreenAreaFlg = true;
bRevisionRequest = true;
}
// 補正指示のエリアを抜けた判定
if (nowGrid != Grid.GreenArea)
{
bGreenAreaFlg = false;
}
}
}
// 動作モードごとの走行情報更新
// ハンドル、アクセルを調整
if (ModeCtrl.GetActionMode() == ModeControl.ActionMode.CheckPoint)
{
// 通常時
// エマージェンシー ブレーキチェック
EBS.EmgBrk = false;
if (null != lrfData && lrfData.Length > 0)
{
//int BrakeLv = CheckEBS(lrfData);
// ノイズ除去モーとで非常停止発動
int BrakeLv = EBS.CheckEBS(LocSys.LRF.getData_UntiNoise(), UpdateCnt);
// 注意Lvに応じた対処(スローダウン指示)
// Lvで段階ごとに分けてるのは、揺れなどによるLRFのノイズ対策
// 瞬間的なノイズでいちいち止まらないように。
if (BrakeLv >= EmergencyBrake.SlowDownLv) EBS.AccelSlowDownCnt = 20;
//if (BrakeLv >= EmergencyBrake.StopLv) EBS.EmgBrk = true; // 緊急停止
}
// ルートにそったハンドル、アクセル値を取得
double handleTgt = RTS.getHandleValue();
double accTgt = RTS.getAccelValue();
// 壁回避
if(useEHS)
{
EHS.HandleVal = EHS.CheckEHS(LocSys.LRF.getData_UntiNoise());
if (0.0 != EHS.HandleVal)
{
// 回避ハンドル操作
// ばたつき防止
handleTgt = CersioCtrl.nowSendHandleValue + ((EHS.HandleVal - CersioCtrl.nowSendHandleValue) * CersioCtrl.HandleControlPow);
EBS.AccelSlowDownCnt = 5; // 速度もさげる
CarCtrl.SetHeadMarkLED((int)CersioCtrl.LED_PATTERN.BLUE);
}
}
// ハンドルで曲がるときは、速度を下げる
if( Math.Abs( handleTgt ) > 0.25 )
{
EBS.AccelSlowDownCnt = 5;
}
// スローダウン中 動作
if (EBS.AccelSlowDownCnt > 0)
{
// 遅くする
accValue = accTgt = RTS.getAccelValue() * EmergencyBrake.AccSlowdownRate;
EBS.AccelSlowDownCnt--;
// ＬＥＤパターンハザード
// スローダウンを知らせる
CarCtrl.SetHeadMarkLED((int)CersioCtrl.LED_PATTERN.HAZERD);
}
else
{
// ＬＥＤパターン平常
CarCtrl.SetHeadMarkLED((int)CersioCtrl.LED_PATTERN.Normal);
}
// 緊急ブレーキ動作
if (EBS.EmgBrk && useEBS)
{
ModeCtrl.SetActionMode(ModeControl.ActionMode.EmergencyStop);
}
handleValue = handleTgt;
accValue = accTgt;
}
else if (ModeCtrl.GetActionMode() == ModeControl.ActionMode.EmergencyStop)
{
// 緊急ブレーキ 中
if (null != lrfData && lrfData.Length > 0)
{
int BrakeLv = EBS.CheckEBS(LocSys.LRF.getData_UntiNoise(), UpdateCnt);
// 停止状態の確認
if (BrakeLv >= EmergencyBrake.StopLv)
{
// 10秒経過したらバック動作へ
if (ModeCtrl.GetModePassSeconds(10))
{
ModeCtrl.SetActionMode(ModeControl.ActionMode.MoveBack);
}
}
else
{
// 緊急ブレーキ解除
ModeCtrl.SetActionMode(ModeControl.ActionMode.CheckPoint);
}
}
// 赤
CarCtrl.SetHeadMarkLED((int)CersioCtrl.LED_PATTERN.RED, true);
accValue = 0.0;
}
else if (ModeCtrl.GetActionMode() == ModeControl.ActionMode.MoveBack)
{
// 復帰（バック）モード
// 向かうべき方向とハンドルを逆に切り、バック
double handleTgt = -RTS.getHandleValue();
//double accTgt = RTS.getAccelValue();
// 障害物をみて、ハンドルを切る方向を求める
{
double ehsHandleVal = EHS.CheckEHS(LocSys.LRF.getData_UntiNoise());
if (useEHS && 0.0 != ehsHandleVal)
{
// 左右に壁を検知してたら、優先的によける
if (EHS.Result == EmergencyHandring.EHS_MODE.LeftWallHit ||
EHS.Result == EmergencyHandring.EHS_MODE.RightWallHit)
{
handleTgt = -ehsHandleVal;
}
}
}
handleValue = handleTgt;
accValue = -EmergencyBrake.AccSlowdownRate; // スローダウンの速度でバック
// ＬＥＤパターン
// バック中
CarCtrl.SetHeadMarkLED((int)CersioCtrl.LED_PATTERN.UnKnown1, true);
// バック完了、回避ルートを動的計算
if (ModeCtrl.GetModePassSeconds(10))
{
ActiveDetour actDetour = new ActiveDetour(LocSys.worldMap.AreaGridMap, LocSys.LRF.getData());
// 現在位置と到達したい位置で、回避ルートを求める
{
int nowPosX, nowPosY;
double nowAng;
int tgtPosX = 0;
int tgtPosY = 0;
RTS.getNowPostion(out nowPosX, out nowPosY, out nowAng);
RTS.getNowTarget(ref tgtPosX, ref tgtPosY);
nowPosX = LocSys.worldMap.GetAreaX(nowPosX);
nowPosY = LocSys.worldMap.GetAreaX(nowPosY);
tgtPosX = LocSys.worldMap.GetAreaX(tgtPosX);
tgtPosY = LocSys.worldMap.GetAreaX(tgtPosY);
// 回避ルートを求める
List<Vector3> avoidCheckPoint = actDetour.Calc_DetourCheckPoint(nowPosX, nowPosY, tgtPosX, tgtPosY);
if (avoidCheckPoint.Count > 0)
{
// 回避ルートあり
// 新規ルートを作成
MapData avoidMap = new MapData();
avoidMap.checkPoint = avoidCheckPoint.ToArray();
avoidMap.MapName = "ActiveDetour AvoidMap";
avoidRTS = new Rooting(avoidMap);
// 回避情報を画面に表示
// 回避イメージ生成
AvoidRootImage = actDetour.getDetourRootImage();
// イメージ表示時間設定
AvoidRootDispTime = DateTime.Now.AddSeconds(15);
// 回避ルートモード
ModeCtrl.SetActionMode(ModeControl.ActionMode.Avoid);
}
else
{
// 回避ルートなし(回避不能)
// ※動作検討
// とりあえず、チェックポイントへ向かう
ModeCtrl.SetActionMode(ModeControl.ActionMode.CheckPoint);
}
}
}
}
UpdateCnt++;
// 回復モードか否か
if (ModeCtrl.GetActionMode() != ModeControl.ActionMode.MoveBack) return false;
return true;
}
public void Reset(string mapFileName)
{
ModeCtrl = new ModeControl();
//
MapFile = MapData.LoadMapFile(mapFileName);
//
RTS = new Rooting(MapFile);
// マップ画像ファイル名、実サイズの横[mm], 実サイズ縦[mm] (北向き基準)
LocSys = new LocPreSumpSystem(MapFile.MapImageFileName, MapFile.RealWidth, MapFile.RealHeight);
UpdateCnt = 0;
goalFlg = false;
handleValue = 0.0;
accValue = 0.0;
Reset_StartPosition();
}
/// <summary>
/// 定期更新(100ms周期)
/// </summary>
/// <param name="LocSys"></param>
/// <param name="useEBS">緊急ブレーキ</param>
/// <param name="useEHS">壁避け動作</param>
/// <param name="bStraightMode">直進モード</param>
/// <returns>true...バック中(緊急動作しない)</returns>
///
public bool Update(LocPreSumpSystem LocSys, bool useEBS, bool useEHS)
{
// LRFデータ取得
double[] lrfData = LocSys.LRF.getData();
// Rooting ------------------------------------------------------------------------------------------------
ModeCtrl.update();
if (ModeCtrl.GetActionMode() == ModeControl.ActionMode.CheckPoint)
{
// ルート進行
// ルート算定
// 現在座標更新
RTS.setNowPostion((int)LocSys.GetResultLocationX(),
(int)LocSys.GetResultLocationY(),
(int)LocSys.GetResultAngle());
// ルート計算
RTS.calcRooting();
// チェックポイント通過をLEDで伝える
if (RTS.IsCheckPointPass())
{
CarCtrl.SetHeadMarkLED((int)CersioCtrl.LED_PATTERN.WHITE_FLASH, true);
}
}
else if (ModeCtrl.GetActionMode() == ModeControl.ActionMode.Avoid)
{
// 回避ルート進行
// ルート算定
// 現在座標更新
avoidRTS.setNowPostion((int)LocSys.GetResultLocationX(),
(int)LocSys.GetResultLocationY(),
(int)LocSys.GetResultAngle());
// ルート計算
avoidRTS.calcRooting();
// 回避中をLEDで伝える
//CarCtrl.SetHeadMarkLED((int)CersioCtrl.LED_PATTERN.WHITE_FLASH, false);
}
// DriveControl -------------------------------------------------------------------------------------------
// マップ情報取得
{
Grid nowGrid = LocSys.GetMapInfo(LocSys.R1);
// マップ情報反映
if (nowGrid == Grid.BlueArea)
{
// スローダウン
EBS.AccelSlowDownCnt = 5;
Brain.addLogMsg += "ColorMap:Blue\n";
}
// 壁の中にいる
if (nowGrid == Grid.RedArea)
{
// 強制補正エリア
Brain.addLogMsg += "LocRivision:ColorMap[Red](マップ情報の位置補正)\n";
bRevisionRequest = true;
}
// 補正実行エリア
{
if (nowGrid == Grid.GreenArea && !bGreenAreaFlg)
{
// 補正指示のエリアに入った
Brain.addLogMsg += "LocRivision:ColorMap[Green](マップ情報の位置補正)\n";
bGreenAreaFlg = true;
bRevisionRequest = true;
}
// 補正指示のエリアを抜けた判定
if (nowGrid != Grid.GreenArea)
{
bGreenAreaFlg = false;
}
}
}
// 動作モードごとの走行情報更新
// ハンドル、アクセルを調整
if (ModeCtrl.GetActionMode() == ModeControl.ActionMode.CheckPoint)
{
// 通常時
// エマージェンシー ブレーキチェック
EBS.EmgBrk = false;
if (null != lrfData && lrfData.Length > 0)
{
//int BrakeLv = CheckEBS(lrfData);
// ノイズ除去モーとで非常停止発動
int BrakeLv = EBS.CheckEBS(LocSys.LRF.getData_UntiNoise(), UpdateCnt);
// 注意Lvに応じた対処(スローダウン指示)
// Lvで段階ごとに分けてるのは、揺れなどによるLRFのノイズ対策
// 瞬間的なノイズでいちいち止まらないように。
if (BrakeLv >= EmergencyBrake.SlowDownLv)
{
EBS.AccelSlowDownCnt = 20;
}
//if (BrakeLv >= EmergencyBrake.StopLv) EBS.EmgBrk = true; // 緊急停止
}
// ルートにそったハンドル、アクセル値を取得
double handleTgt = RTS.getHandleValue();
double accTgt = RTS.getAccelValue();
// 壁回避
if (useEHS)
{
EHS.HandleVal = EHS.CheckEHS(LocSys.LRF.getData_UntiNoise());
if (0.0 != EHS.HandleVal)
{
// 回避ハンドル操作
// ばたつき防止
handleTgt = CersioCtrl.nowSendHandleValue + ((EHS.HandleVal - CersioCtrl.nowSendHandleValue) * CersioCtrl.HandleControlPow);
EBS.AccelSlowDownCnt = 5; // 速度もさげる
CarCtrl.SetHeadMarkLED((int)CersioCtrl.LED_PATTERN.BLUE);
}
}
// ハンドルで曲がるときは、速度を下げる
if (Math.Abs(handleTgt) > 0.25)
{
EBS.AccelSlowDownCnt = 5;
}
// スローダウン中 動作
if (EBS.AccelSlowDownCnt > 0)
{
// 遅くする
accValue = accTgt = RTS.getAccelValue() * EmergencyBrake.AccSlowdownRate;
EBS.AccelSlowDownCnt--;
// ＬＥＤパターンハザード
// スローダウンを知らせる
CarCtrl.SetHeadMarkLED((int)CersioCtrl.LED_PATTERN.HAZERD);
}
else
{
// ＬＥＤパターン平常
CarCtrl.SetHeadMarkLED((int)CersioCtrl.LED_PATTERN.Normal);
}
// 緊急ブレーキ動作
if (EBS.EmgBrk && useEBS)
{
ModeCtrl.SetActionMode(ModeControl.ActionMode.EmergencyStop);
}
handleValue = handleTgt;
accValue = accTgt;
}
else if (ModeCtrl.GetActionMode() == ModeControl.ActionMode.EmergencyStop)
{
// 緊急ブレーキ 中
if (null != lrfData && lrfData.Length > 0)
{
int BrakeLv = EBS.CheckEBS(LocSys.LRF.getData_UntiNoise(), UpdateCnt);
// 停止状態の確認
if (BrakeLv >= EmergencyBrake.StopLv)
{
// 10秒経過したらバック動作へ
if (ModeCtrl.GetModePassSeconds(10))
{
ModeCtrl.SetActionMode(ModeControl.ActionMode.MoveBack);
}
}
else
{
// 緊急ブレーキ解除
ModeCtrl.SetActionMode(ModeControl.ActionMode.CheckPoint);
}
}
// 赤
CarCtrl.SetHeadMarkLED((int)CersioCtrl.LED_PATTERN.RED, true);
accValue = 0.0;
}
else if (ModeCtrl.GetActionMode() == ModeControl.ActionMode.MoveBack)
{
// 復帰（バック）モード
// 向かうべき方向とハンドルを逆に切り、バック
double handleTgt = -RTS.getHandleValue();
//double accTgt = RTS.getAccelValue();
// 障害物をみて、ハンドルを切る方向を求める
{
double ehsHandleVal = EHS.CheckEHS(LocSys.LRF.getData_UntiNoise());
if (useEHS && 0.0 != ehsHandleVal)
{
// 左右に壁を検知してたら、優先的によける
if (EHS.Result == EmergencyHandring.EHS_MODE.LeftWallHit ||
EHS.Result == EmergencyHandring.EHS_MODE.RightWallHit)
{
handleTgt = -ehsHandleVal;
}
}
}
handleValue = handleTgt;
accValue = -EmergencyBrake.AccSlowdownRate; // スローダウンの速度でバック
// ＬＥＤパターン
// バック中
CarCtrl.SetHeadMarkLED((int)CersioCtrl.LED_PATTERN.UnKnown1, true);
// バック完了、回避ルートを動的計算
if (ModeCtrl.GetModePassSeconds(10))
{
ActiveDetour actDetour = new ActiveDetour(LocSys.worldMap.AreaGridMap, LocSys.LRF.getData());
// 現在位置と到達したい位置で、回避ルートを求める
{
int nowPosX, nowPosY;
double nowAng;
int tgtPosX = 0;
int tgtPosY = 0;
RTS.getNowPostion(out nowPosX, out nowPosY, out nowAng);
RTS.getNowTarget(ref tgtPosX, ref tgtPosY);
nowPosX = LocSys.worldMap.GetAreaX(nowPosX);
nowPosY = LocSys.worldMap.GetAreaX(nowPosY);
tgtPosX = LocSys.worldMap.GetAreaX(tgtPosX);
tgtPosY = LocSys.worldMap.GetAreaX(tgtPosY);
// 回避ルートを求める
List <Vector3> avoidCheckPoint = actDetour.Calc_DetourCheckPoint(nowPosX, nowPosY, tgtPosX, tgtPosY);
if (avoidCheckPoint.Count > 0)
{
// 回避ルートあり
// 新規ルートを作成
MapData avoidMap = new MapData();
avoidMap.checkPoint = avoidCheckPoint.ToArray();
avoidMap.MapName = "ActiveDetour AvoidMap";
avoidRTS = new Rooting(avoidMap);
// 回避情報を画面に表示
// 回避イメージ生成
AvoidRootImage = actDetour.getDetourRootImage();
// イメージ表示時間設定
AvoidRootDispTime = DateTime.Now.AddSeconds(15);
// 回避ルートモード
ModeCtrl.SetActionMode(ModeControl.ActionMode.Avoid);
}
else
{
// 回避ルートなし(回避不能)
// ※動作検討
// とりあえず、チェックポイントへ向かう
ModeCtrl.SetActionMode(ModeControl.ActionMode.CheckPoint);
}
}
}
}
UpdateCnt++;
// 回復モードか否か
if (ModeCtrl.GetActionMode() != ModeControl.ActionMode.MoveBack)
{
return(false);
}
return(true);
}
