示例#1
0
        /// <summary>
        ///
        /// </summary>
        /// <param name="setPattern"></param>
        /// <param name="bForce">強制変更</param>
        /// <returns>変更したor済み..True</returns>
        public bool SetHeadMarkLED(CersioCtrl carCtrl, int setPattern, bool bForce = false)
        {
            if (bForce || (ptnHeadLED != setPattern && cntHeadLED == 0))
            {
                carCtrl.SendCommand("AL," + setPattern.ToString() + ",\n");

                cntHeadLED = 20 * 1;          // しばらく変更しない
                ptnHeadLED = setPattern;
                return(true);
            }


            // 通常以外は、現在設定中でさらに送られてきたら延長
            if (setPattern != (int)LED_PATTERN.Normal && ptnHeadLED == setPattern)
            {
                cntHeadLED = 10 * 1;          // 占有時間延長
                return(true);
            }
            return(false);
        }
示例#2
0
        // ===========================================================================================================

        /// <summary>
        /// コンストラクタ
        /// </summary>
        /// <param name="ceCtrl"></param>
        public Brain(CersioCtrl ceCtrl, string mapFileName )
        {
            CarCtrl = ceCtrl;
            Reset(mapFileName);
        }
 /// <summary>
 /// ログ用のバッファクリア
 /// </summary>
 /// <param name="BrainCtrl"></param>
 /// <param name="CersioCt"></param>
 /// <param name="LocSys"></param>
 public void LogBuffer_Clear(ref Brain BrainCtrl, ref CersioCtrl CersioCt)
 {
     if (null != CersioCt)
     {
         CersioCt.hwSendStr = "";
         CersioCt.hwResiveStr = "";
     }
 }
        /// <summary>
        /// VehicleRunnerログ出力
        /// </summary>
        public void Output_VRLog(ref Brain BrainCtrl, ref CersioCtrl CersioCt)
        {
            System.IO.StreamWriter sw = new System.IO.StreamWriter(saveLogFname, true, System.Text.Encoding.GetEncoding("shift_jis"));

            LocPreSumpSystem LocSys = BrainCtrl.LocSys;

            // 固有識別子 + 時間
            sw.Write("LocPresumpLog:" + DateTime.Now.ToString("yyyy/MM/dd HH:mm:ss.fff") + System.Environment.NewLine);

            // ハードウェア情報
            if (CersioCt.TCP_IsConnected())
            {
                if (null != CersioCt.hwSendStr)
                {
                    sw.Write("hwSendStr:" + CersioCt.hwSendStr.Replace('\n', ' ') + System.Environment.NewLine);
                }
                if (null != CersioCt.hwResiveStr)
                {
                    sw.Write("hwResiveStr:" + CersioCt.hwResiveStr + System.Environment.NewLine);
                }
                sw.Write("handle:" + CersioCtrl.nowSendHandleValue + " / acc:" + CersioCtrl.nowSendAccValue + System.Environment.NewLine);
            }
            else
            {
                sw.Write("Comment:No Connect BoxPC" + System.Environment.NewLine);
            }

            // 位置情報
            {
                sw.Write("R1:X " + LocSys.R1.X.ToString("f3") +
                         "/Y " + LocSys.R1.Y.ToString("f3") +
                         "/ Dir " + LocSys.R1.Theta.ToString("f2") +
                         System.Environment.NewLine);

                sw.Write("V1:X " + LocSys.V1.X.ToString("f3") +
                         "/Y " + LocSys.V1.Y.ToString("f3") +
                         "/ Dir " + LocSys.V1.Theta.ToString("f2") +
                         System.Environment.NewLine);

                sw.Write("E1:X " + LocSys.E1.X.ToString("f3") +
                         "/Y " + LocSys.E1.Y.ToString("f3") +
                         "/ Dir " + LocSys.E1.Theta.ToString("f2") +
                         System.Environment.NewLine);

                sw.Write("C1:X " + LocSys.C1.X.ToString("f3") +
                         "/Y " + LocSys.C1.Y.ToString("f3") +
                         "/ Dir " + LocSys.C1.Theta.ToString("f2") +
                         System.Environment.NewLine);

                sw.Write("G1:X " + LocSys.G1.X.ToString("f3") +
                         "/Y " + LocSys.G1.Y.ToString("f3") +
                         "/ Dir " + LocSys.G1.Theta.ToString("f2") +
                         System.Environment.NewLine);
            }

            //
            {
                // Rooting情報
                {
                    sw.Write("RTS_TargetIndex:" + BrainCtrl.RTS.GetNowCheckPointIdx() + System.Environment.NewLine);

                    int tgtPosX = 0;
                    int tgtPosY = 0;
                    double tgtDir = 0;
                    BrainCtrl.RTS.getNowTarget(ref tgtPosX, ref tgtPosY);
                    BrainCtrl.RTS.getNowTargetDir(ref tgtDir);

                    sw.Write("RTS_TargetPos:X " + tgtPosX.ToString("f3") +
                             "/Y " + tgtPosY.ToString("f3") +
                             "/Dir " + tgtDir.ToString("f2") +
                             System.Environment.NewLine);

                    sw.Write("RTS_Handle:" + BrainCtrl.RTS.getHandleValue().ToString("f2") + System.Environment.NewLine);
                }

                // Brain情報
                {
                    sw.Write("EBS_CautionLv:" + BrainCtrl.EBS.CautionLv.ToString() + System.Environment.NewLine);
                    sw.Write("EHS_Handle:" + BrainCtrl.EHS.HandleVal.ToString("f2") + "/Result " + EmergencyHandring.ResultStr[(int)BrainCtrl.EHS.Result] + System.Environment.NewLine);
                }

                // CersioCtrl
                {
                    sw.Write("GoalFlg:" + (BrainCtrl.goalFlg ? "TRUE" : "FALSE") + System.Environment.NewLine);

                    if (CersioCt.bhwCompass)
                    {
                        sw.Write("hwCompass:"******"hwREPlot:X " + CersioCt.hwREX.ToString("f3") +
                                 "/Y " + CersioCt.hwREY.ToString("f3") +
                                 "/Dir " + CersioCt.hwREDir.ToString("f2") +
                                 System.Environment.NewLine);
                    }

                    if (CersioCt.bhwGPS)
                    {
                        sw.Write("hwGPS:X " + CersioCt.hwGPS_LandX.ToString("f5") +
                                 "/Y " + CersioCt.hwGPS_LandY.ToString("f5") +
                                 "/Dir " + CersioCt.hwGPS_MoveDir.ToString("f2") +
                                 System.Environment.NewLine);
                    }
                }

                // 特記事項メッセージ出力
                if (Brain.addLogMsg != null)
                {
                    sw.Write("AddLog:" + Brain.addLogMsg + System.Environment.NewLine);
                }
                Brain.addLogMsg = null;
            }
            // 改行
            sw.Write(System.Environment.NewLine);

            //閉じる
            sw.Close();
        }
        /// <summary>
        /// インジケータ描画
        /// </summary>
        /// <param name="g"></param>
        /// <param name="baseY"></param>
        public void DrawIngicator(Graphics g, int baseY, ref CersioCtrl CersioCt, ref Brain BrainCtrl)
        {
            {
                // Handle
                int stX = 20;
                int stY = baseY + 8;
                int Wd = 200;
                int Ht = 15;

                float handleVal = (float)((Wd / 2) * (-CersioCtrl.nowSendHandleValue));
                if (handleVal > 0)
                {
                    g.FillRectangle(Brushes.Red, stX + Wd / 2, stY, handleVal, Ht);
                }
                else
                {
                    g.FillRectangle(Brushes.Red, stX + Wd / 2 + handleVal, stY, -handleVal, Ht);
                }
                g.DrawRectangle(Pens.White, stX, stY, Wd, Ht);
                g.DrawLine(Pens.White, stX + Wd / 2, stY, stX + Wd / 2, stY + Ht);
                g.DrawString("Handle", fntMini, Brushes.White, stX + Wd / 2 - 38 / 2, stY + Ht - fntMini.GetHeight());

                // ACC
                stX = 40;
                stY = baseY + 30;
                Wd = 10;
                Ht = 40;

                float accVal = (float)(Ht * CersioCtrl.nowSendAccValue);
                g.FillRectangle(Brushes.Red, stX, stY + Ht - accVal, Wd, accVal);
                g.DrawRectangle(Pens.White, stX, stY, Wd, Ht);
                g.DrawString("Acc", fntMini, Brushes.White, stX - 35, stY + Ht - fntMini.GetHeight());

            }

            // Compus
            if (CersioCt.bhwCompass)
            {
                int ang = CersioCt.hwCompass;
                DrawMaker(g, Brushes.Red, 90, baseY + 50, ang, 12);
                g.DrawString(ang.ToString(), fntMini, Brushes.White, 90 - 25, baseY + 50);
                g.DrawString("Compus", fntMini, Brushes.White, 90 - 25, baseY + 50 + 14);
            }

            // RE Dir
            if (CersioCt.bhwREPlot)
            {
                double ang = CersioCt.hwREDir;
                DrawMaker(g, Brushes.Purple, 150, baseY + 50, ang, 12);
                g.DrawString(ang.ToString("F1"), fntMini, Brushes.White, 150 - 25, baseY + 50);
                g.DrawString("R.E.Dir", fntMini, Brushes.White, 150 - 25, baseY + 50 + 14);
            }

            // GPS Dir
            if (CersioCt.bhwGPS)
            {
                if (CersioCt.bhwUsbGPS)
                {
                    // USBGPSは角度あり
                    DrawMaker(g, Brushes.LimeGreen, 210, baseY + 50, CersioCt.hwGPS_MoveDir, 12);
                }
                else
                {
                    // bServerGPS 角度なし
                    DrawMaker(g, Brushes.LimeGreen, 210, baseY + 50, 12);
                }
                g.DrawString("GPSDir", fntMini, Brushes.White, 210 - 25, baseY + 50 + 14);
            }

            // 向けたいハンドル角度
            {
                double ang = BrainCtrl.RTS.getNowTargetStearingDir();
                DrawMaker(g, Brushes.Cyan, 40, baseY + 120, ang, 12);
                g.DrawString(ang.ToString("F1"), fntMini, Brushes.White, 40 - 25, baseY + 120);
                g.DrawString("Handle", fntMini, Brushes.White, 40 - 25, baseY + 120 + 14);
            }

            // 現在の向き
            {
                double ang = BrainCtrl.RTS.getNowDir();
                DrawMaker(g, Brushes.Red, 100, baseY + 120, ang, 12);
                g.DrawString(ang.ToString("F1"), fntMini, Brushes.White, 100 - 25, baseY + 120);
                g.DrawString("RTSNow", fntMini, Brushes.White, 100 - 25, baseY + 120 + 14);
            }

            // 相手の向き
            {
                double ang = BrainCtrl.RTS.getNowTargetDir();
                DrawMaker(g, Brushes.Purple, 160, baseY + 120, ang, 12);
                g.DrawString(ang.ToString("F1"), fntMini, Brushes.White, 160 - 25, baseY + 120);
                g.DrawString("RTSTgt", fntMini, Brushes.White, 160 - 25, baseY + 120 + 14);
            }
        }
        //static int areaMapDrawCnt = 0;
        public void AreaMap_Draw_WorldMap(Graphics g, ref CersioCtrl CersioCt, ref Brain BrainCtrl)
        {
            LocPreSumpSystem LocSys = BrainCtrl.LocSys;

            // 全体マップ描画
            float viewScale;

            g.FillRectangle(Brushes.Black, 0, 0, worldMapBmp.Width, worldMapBmp.Height);

            if (((float)LocSys.worldMap.WorldSize.w / (float)worldMapBmp.Width) < ((float)LocSys.worldMap.WorldSize.h / (float)worldMapBmp.Height))
            {
                viewScale = (float)(1.0 / ((float)LocSys.worldMap.WorldSize.h / (float)worldMapBmp.Height));
            }
            else
            {
                viewScale = (float)(1.0 / ((float)LocSys.worldMap.WorldSize.w / (float)worldMapBmp.Width));
            }

            //g.ResetTransform();
            //g.TranslateTransform(-ctrX, -ctrY, MatrixOrder.Append);
            //g.RotateTransform((float)layer.wAng, MatrixOrder.Append);
            //g.ScaleTransform(viewScale, viewScale, MatrixOrder.Append);

            if (null != worldMapBmp)
            {
                g.DrawImage(worldMapBmp, 0, 0);
            }

            //g.ResetTransform();

            // 各マーカーの位置を描画
            LocSys.DrawWorldMap(g, viewScale);

            // ターゲット描画
            if (null != CersioCt)
            {
                int tgtPosX, tgtPosY;
                double dir = 0;
                tgtPosX = tgtPosY = 0;

                BrainCtrl.RTS.getNowTarget(ref tgtPosX, ref tgtPosY);
                BrainCtrl.RTS.getNowTargetDir(ref dir);
                MarkPoint tgtMk = new MarkPoint(tgtPosX, tgtPosY, dir + 180);

                DrawMaker(g, viewScale, tgtMk, Brushes.GreenYellow, 8);

                // ターゲットまでのライン
                DrawMakerLine(g, viewScale,
                    LocSys.R1,
                    tgtMk,
                    Pens.Olive, 1);
            }
        }
        /// <summary>
        /// エリアマップ描画
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        public void AreaMap_Draw_Area(Graphics g, ref CersioCtrl CersioCt, ref Brain BrainCtrl)
        {
            // 書き換えBMP(追加障害物)描画
            LocPreSumpSystem LocSys = BrainCtrl.LocSys;

            // エリアマップ描画
            //g.FillRectangle(Brushes.Black, 0, 0, picbox_AreaMap.Width, picbox_AreaMap.Height);

            g.DrawImage(LocSys.AreaOverlayBmp, 0, 0);

            // ターゲット描画
            if (null != CersioCt)
            {
                int tgtPosX, tgtPosY;
                double dir = 0;
                tgtPosX = tgtPosY = 0;
                float olScale = (float)LocSys.AreaOverlayBmp.Width / (float)LocSys.AreaBmp.Width;
                BrainCtrl.RTS.getNowTarget(ref tgtPosX, ref tgtPosY);
                BrainCtrl.RTS.getNowTargetDir(ref dir);
                MarkPoint tgtMk = new MarkPoint(LocSys.worldMap.GetAreaX(tgtPosX), LocSys.worldMap.GetAreaY(tgtPosY), dir);

                DrawMaker(g, olScale, tgtMk, Brushes.GreenYellow, 8);

                // ターゲットまでのライン
                DrawMakerLine(g, olScale,
                    new MarkPoint(LocSys.worldMap.GetAreaX(LocSys.R1.X), LocSys.worldMap.GetAreaY(LocSys.R1.Y), 0),
                    tgtMk,
                    Pens.Olive, 1);
            }
        }
        // ================================================================================================================

        /// <summary>
        /// コンストラクタ
        /// </summary>
        public VehicleRunnerForm()
        {
            InitializeComponent();

            formLog.init();

            // セルシオコントローラ初期化
            CersioCt = new CersioCtrl();

            // BoxPc(bServer) Emu接続
            // 起動時にレスポンスの早いエミュレータをつなぎにいく
            // エミュレータがなければ、リトライ時 実機のbServerにつなぎにいく
            //bServerConnectAsync();
            CersioCt.ConnectBoxPC_Emulator();

            // ブレイン起動
            BrainCtrl = new Brain(CersioCt, defaultMapFile);

            // 初期ボタン設定セット
            rb_Move_CheckedChanged(null, null);
            rb_Dir_CheckedChanged(null, null);

            // 自己位置補正設定 切り替え
            cb_VRRevision_CheckedChanged(null, null);

            // マップウィンドウサイズのbmp作成
            formDraw.MakePictureBoxWorldMap(BrainCtrl.LocSys.worldMap.mapBmp, picbox_AreaMap);

            // LRF 入力スケール調整反映
            tb_LRFScale.Text = trackBar_LRFViewScale.Value.ToString();
            //btm_LRFScale_Click(null, null);
            tb_LRFScale_TextChanged(null, null);

            // センサー値取得 スレッド起動
            Thread trdSensorLRF = new Thread(new ThreadStart(ThreadSensorUpdate_LRF));
            trdSensorLRF.IsBackground = true;   // アプリの終了とともに終了する
            trdSensorLRF.Priority = ThreadPriority.AboveNormal;
            trdSensorLRF.Start();

            Thread trdSensor = new Thread(new ThreadStart(ThreadSensorUpdate_bServer));
            trdSensor.IsBackground = true;
            trdSensor.Priority = ThreadPriority.AboveNormal;
            trdSensor.Start();

            // 位置座標更新 スレッド起動
            /*
            Thread trdLocalize = new Thread(new ThreadStart(ThreadLocalizationUpdate));
            trdLocalize.IsBackground = true;
            //trdSensor.Priority = ThreadPriority.AboveNormal;
            trdLocalize.Start();
            */

#if EMULATOR_MODE
            // LRF エミュレーション
            tb_LRFIpAddr.Text = "127.0.0.10";
#endif

        }