/// <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--; // LEDパターンハザード // スローダウンを知らせる CarCtrl.SetHeadMarkLED((int)CersioCtrl.LED_PATTERN.HAZERD); } else { // LEDパターン平常 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; // スローダウンの速度でバック // LEDパターン // バック中 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; }
/// <summary> /// 緊急停止情報のLRFデータ /// </summary> /// <param name="g"></param> /// <param name="LocSys"></param> /// <param name="BrainCtrl"></param> /// <param name="lrfdata"></param> /// <param name="ctrX"></param> /// <param name="ctrY"></param> /// <param name="scale"></param> /// <param name="picScale"></param> public void LRF_Draw_PointEBS(Graphics g, ref LocPreSumpSystem LocSys, ref Brain BrainCtrl, double[] lrfdata, int ctrX, int ctrY, double scale, double picScale) { { int stAng = EmergencyBrake.stAng + (int)BrainCtrl.EBS.HandleDiffAngle; int edAng = EmergencyBrake.edAng + (int)BrainCtrl.EBS.HandleDiffAngle; int cirSize; if (BrainCtrl.EBS.CautionLv >= EmergencyBrake.StopLv) { // ブレーキレンジ内 Brush colBrs = Brushes.Red; cirSize = (int)((EmergencyBrake.BrakeRange * 2.0 / LocSys.MapToRealScale) * scale); g.FillPie(colBrs, (ctrX - cirSize / 2), (ctrY - cirSize / 2), cirSize, cirSize, stAng - 90, (edAng - stAng)); } else if (BrainCtrl.EBS.CautionLv >= EmergencyBrake.SlowDownLv) { // スローダウンレンジ内 Brush colBrs = Brushes.Orange; cirSize = (int)((EmergencyBrake.SlowRange * 2.0 / LocSys.MapToRealScale) * scale); g.FillPie(colBrs, (ctrX - cirSize / 2), (ctrY - cirSize / 2), cirSize, cirSize, stAng - 90, (edAng - stAng)); } { Pen colPen = Pens.Yellow; // スローダウン レンジ枠 cirSize = (int)((EmergencyBrake.SlowRange * 2.0 / LocSys.MapToRealScale) * scale); g.DrawPie(colPen, (ctrX - cirSize / 2), (ctrY - cirSize / 2), cirSize, cirSize, stAng - 90, (edAng - stAng)); // ブレーキ レンジ枠 cirSize = (int)((EmergencyBrake.BrakeRange * 2.0 / LocSys.MapToRealScale) * scale); g.DrawPie(colPen, (ctrX - cirSize / 2), (ctrY - cirSize / 2), cirSize, cirSize, stAng - 90, (edAng - stAng)); } } // EHS範囲描画 { int stAng; int edAng; int cirSize = (int)((EmergencyHandring.MaxRange * 2.0 / LocSys.MapToRealScale) * scale); Pen colPen = Pens.LightGreen; // 左側 stAng = EmergencyHandring.stLAng; edAng = EmergencyHandring.edLAng; if (BrainCtrl.EHS.Result == EmergencyHandring.EHS_MODE.LeftWallHit || BrainCtrl.EHS.Result == EmergencyHandring.EHS_MODE.CenterPass) { g.FillPie(Brushes.Red, (ctrX - cirSize / 2), (ctrY - cirSize / 2), cirSize, cirSize, -stAng - 90, -(edAng - stAng)); } g.DrawPie(colPen, (ctrX - cirSize / 2), (ctrY - cirSize / 2), cirSize, cirSize, -stAng - 90, -(edAng - stAng)); // 右側 stAng = EmergencyHandring.stRAng; edAng = EmergencyHandring.edRAng; if (BrainCtrl.EHS.Result == EmergencyHandring.EHS_MODE.RightWallHit || BrainCtrl.EHS.Result == EmergencyHandring.EHS_MODE.CenterPass) { g.FillPie(Brushes.Red, (ctrX - cirSize / 2), (ctrY - cirSize / 2), cirSize, cirSize, -stAng - 90, -(edAng - stAng)); } g.DrawPie(colPen, (ctrX - cirSize / 2), (ctrY - cirSize / 2), cirSize, cirSize, -stAng - 90, -(edAng - stAng)); } // ノイズリダクションLRF描画 if (lrfdata != null) { LRF_Draw_Point(g, Brushes.Cyan, lrfdata, ctrX, ctrY, picScale*(1.0 / LocSys.MapToRealScale)); } }
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> /// LRF用のガイドライン描画 /// </summary> /// <param name="g"></param> /// <param name="LocSys"></param> /// <param name="ctrX"></param> /// <param name="ctrY"></param> /// <param name="scale"></param> public void LRF_Draw_GuideLine(Graphics g, ref LocPreSumpSystem LocSys, int ctrX, int ctrY, double scale ) { // ガイド描画 // 30mを2m区切りで描画 for (int i = 1; i <= 30 / 2; i++) { int cirSize = (int)((((i * 2000) * 2) / LocSys.MapToRealScale) * scale); g.DrawPie(Pens.LightGray, (ctrX - cirSize / 2), (ctrY - cirSize / 2), cirSize, cirSize, -135 - 90, 270); g.DrawString((i*2).ToString("F1")+"m", fntMini, Brushes.LightGray, (ctrX - cirSize / 2), (ctrY - cirSize / 2)); } }