/// <summary>
/// 描画と親密な定期処理
/// </summary>
/// <param name="sender"></param>
/// <param name="e"></param>
private void tm_Update_Tick(object sender, EventArgs e)
{
if (null != BrainCtrl && null != BrainCtrl.LocSys)
{
LocationSystem LocSys = BrainCtrl.LocSys;
// tm_UpdateHw_Tick
{
// 実走行時、bServerと接続が切れたら再接続
if (updateHwCnt % 100 == 0)
{
// 切断状態なら、自動接続
if (!CersioCt.TCP_IsConnected())
{
//
//CersioCt.Connect_bServer_Async(bServerEmuAddr);
CersioCt.Connect_bServer_Async(bServerAddr);
}
}
// ロータリーエンコーダ（タイヤ回転数）情報
if (CersioCt.bhwRE)
{
//lbl_RErotR.Text = CersioCt.hwRErotR.ToString("f1");
//lbl_RErotL.Text = CersioCt.hwRErotL.ToString("f1");
dataGridViewReceiveData.Rows[(int)GeridRow.RE].Cells[1].Value = "L:" + CersioCt.hwRErotL.ToString("f1");
dataGridViewReceiveData.Rows[(int)GeridRow.RE].Cells[2].Value = "R:" + CersioCt.hwRErotR.ToString("f1");
}
// AMCL
LocSys.Input_ROSPosition(CersioCt.hwAMCL_X, CersioCt.hwAMCL_Y, CersioCt.hwAMCL_Ang);
if (CersioCt.bhwTrgAMCL)
{
// 受信再開時の初期化
LocSys.Reset_ROSPosition(CersioCt.hwAMCL_X, CersioCt.hwAMCL_Y, CersioCt.hwAMCL_Ang);
CersioCt.bhwTrgAMCL = false;
}
// VehicleRunner Plot
{
PointD wlPos = new PointD();
REncoderToMap.CalcWheelPlotXY(ref wlR, ref wlL, ref reAng,
CersioCt.hwRErotR,
CersioCt.hwRErotL,
reOldR,
reOldL);
wlPos.X = (wlR.X + wlL.X) * 0.5;
wlPos.Y = (wlR.Y + wlL.Y) * 0.5;
LocSys.Input_REPosition(wlPos.X, wlPos.Y, -reAng);
reOldR = CersioCt.hwRErotR;
reOldL = CersioCt.hwRErotL;
}
// LED状態 画面表示
if (CersioCt.LEDCtrl.ptnHeadLED == -1)
{
//lbl_LED.Text = "ND";
dataGridViewReceiveData.Rows[(int)GeridRow.LED].Cells[1].Value = "None";
}
else
{
string ledStr = CersioCt.LEDCtrl.ptnHeadLED.ToString();
if (CersioCt.LEDCtrl.ptnHeadLED >= 0 && CersioCt.LEDCtrl.ptnHeadLED < LEDControl.LEDMessage.Count())
{
ledStr += "," + LEDControl.LEDMessage[CersioCt.LEDCtrl.ptnHeadLED];
}
dataGridViewReceiveData.Rows[(int)GeridRow.LED].Cells[1].Value = ledStr;
/*
* if (!ledStr.Equals(lbl_LED.Text))
* {
* lbl_LED.Text = ledStr;
* }*/
}
// 現在座標 表示
//lbl_REPlotX.Text = LocSys.GetResultLocationX().ToString("F2");
//lbl_REPlotY.Text = LocSys.GetResultLocationY().ToString("F2");
//lbl_REPlotDir.Text = LocSys.GetResultAngle().ToString("F2");
dataGridViewReceiveData.Rows[(int)GeridRow.Plot].Cells[1].Value = LocSys.GetResultLocationX().ToString("F2");
dataGridViewReceiveData.Rows[(int)GeridRow.Plot].Cells[2].Value = LocSys.GetResultLocationY().ToString("F2");
dataGridViewReceiveData.Rows[(int)GeridRow.Plot].Cells[3].Value = LocSys.GetResultAngle().ToString("F2");
// CheckPointIndex
if (BrainCtrl.LocSys.RTS.TrgCheckPoint())
{
// チェックポイント通過時に表示更新
numericUD_CheckPoint.Value = BrainCtrl.LocSys.RTS.GetCheckPointIdx();
}
// BoxPC接続状態確認
if (CersioCt.TCP_IsConnected())
{
// 接続ＯＫ
tb_SendData.BackColor = Color.Lime;
tb_ResiveData.BackColor = Color.Lime;
lb_BServerConnect.Text = "bServer [" + CersioCt.TCP_GetConnectedAddr() + "] 接続OK";
lb_BServerConnect.BackColor = Color.Lime;
}
else
{
// 接続ＮＧ
tb_SendData.BackColor = SystemColors.Window;
tb_ResiveData.BackColor = SystemColors.Window;
lb_BServerConnect.Text = "bServer 未接続";
lb_BServerConnect.BackColor = SystemColors.Window;
}
// 送受信文字 画面表示
if (!string.IsNullOrEmpty(CersioCt.hwResiveStr))
{
tb_ResiveData.Text = CersioCt.hwResiveStr.Replace('\n', ' ');
CersioCt.hwResiveStr = "";
}
if (!string.IsNullOrEmpty(CersioCt.hwSendStr))
{
tb_SendData.Text = CersioCt.hwSendStr.Replace('\n', ' ');
CersioCt.hwSendStr = "";
}
updateHwCnt++;
}
// マップ上の現在位置更新
// 現在位置を、AMCL,REPlotどちらを使うか
LocationSystem.LOCATION_SENSOR selSensor = (rb_SelAMCL.Checked ? LocationSystem.LOCATION_SENSOR.AMCL : LocationSystem.LOCATION_SENSOR.REPlot);
BrainCtrl.LocSys.update_NowLocation(selSensor);
// 自律走行(緊急ブレーキ、壁よけ含む)処理 更新
double speedKmh = (double)numericUpDownCtrlSpeed.Value;
BrainCtrl.AutonomousProc(bRunAutonomous, cb_MoveBaseControl.Checked, speedKmh);
// 距離計
//tb_Trip.Text = (LocSys.GetResultDistance_mm() * (1.0 / 1000.0)).ToString("f2");
tb_Trip.Text = (CersioCt.nowLengthMm * (1.0 / 1000.0)).ToString("f2");
}
// スピード表示
tb_RESpeed.Text = ((CersioCt.nowSpeedMmSec * 3600.0) / (1000.0 * 1000.0)).ToString("f2"); // Km/Hour
//tb_RESpeed.Text = (CersioCt.SpeedMmSec).ToString("f2"); // mm/Sec
// ACコマンド送信した ハンドル、アクセル値 表示
tb_AccelVal.Text = CersioCt.sendAccelValue.ToString("f2");
tb_HandleVal.Text = CersioCt.sendHandleValue.ToString("f2");
//labelMoveBaseX.Text = CersioCt.hwMVBS_X.ToString("f2");
//labelMoveBaseAng.Text = CersioCt.hwMVBS_Ang.ToString("f2");
// グリッド表示
dataGridViewReceiveData.Rows[(int)GeridRow.MoveBase].Cells[1].Value = CersioCt.hwMVBS_X.ToString("f2");
dataGridViewReceiveData.Rows[(int)GeridRow.MoveBase].Cells[2].Value = CersioCt.hwMVBS_Y.ToString("f2");
dataGridViewReceiveData.Rows[(int)GeridRow.MoveBase].Cells[3].Value = CersioCt.hwMVBS_Ang.ToString("f2");
dataGridViewReceiveData.Rows[(int)GeridRow.CmdVel].Cells[1].Value = CersioCt.nowAccValue.ToString("f2");
dataGridViewReceiveData.Rows[(int)GeridRow.CmdVel].Cells[2].Value = "0.00";
dataGridViewReceiveData.Rows[(int)GeridRow.CmdVel].Cells[3].Value = CersioCt.nowHandleValue.ToString("f2");
dataGridViewReceiveData.Rows[(int)GeridRow.ActionMode].Cells[1].Value = BrainCtrl.ModeCtrl.GetActionModeString();
dataGridViewReceiveData.Rows[(int)GeridRow.StackCnt].Cells[1].Value = BrainCtrl.noPowerTrainCnt.ToString();
dataGridViewReceiveData.Rows[(int)GeridRow.StackCnt].Cells[2].Value = BrainCtrl.noFowrdCnt.ToString();
// 自律走行情報
if (bRunAutonomous)
{
// 動作内容TextBox表示
lbl_ActionMode.Text = "自律走行[" + BrainCtrl.ModeCtrl.GetActionModeString() + "]";
}
else
{
lbl_ActionMode.Text = "モニタリング モード";
}
// 画面描画
PictureUpdate();
}