/// <summary>
/// 状態リストにデータを追加
/// </summary>
/// <param name="log">ログデータ</param>
public void AppendList(NxtLog log)
{
state.UpdateState(log);
stateLogList.Add((NxtState)state.Clone());
stateTextBuilder.AppendFormat(state.ToString());
}
/// <summary>
/// テキストボックスへログデータを追加
/// </summary>
private void AppendStateTextBox(NxtLog logMessage)
{
bufStateTextBox += stateManager.CurrentState.ToString();
// テキストボックスへの書込み（AppendText）を頻繁に繰り返すと
// 実行速度が低下するためバッファーを介して書込みをまとめて行う。
// ストップウォッチ停止
txtStateOutputStopwatch.Stop();
// 更新時間が一定時間内であればスキップ
if (txtStateOutputStopwatch.ElapsedMilliseconds > 20)
{
// テキストボックスへ追記
txtStateOutput.AppendText(bufStateTextBox);
// 最終行までスクロール
txtStateOutput.ScrollToEnd();
// 書込みバッファーをクリア
bufStateTextBox = String.Empty;
// ストップウォッチをリセット
txtStateOutputStopwatch.Reset();
}
// ストップウオッチを（再）スタート
txtStateOutputStopwatch.Start();
}
/// <summary>
/// 時間を更新
/// </summary>
/// <param name="log">Log</param>
private void UpdateTime(NxtLog log)
{
uint time = 0;
uint momentTime = 0;
// 1回目
if (RunTimeLog.Count == 0)
{
// 開始時間を設定
startTime = log.RelTick;
}
// 2回目以降
else
{
// 走行時間T[ms] = システム時刻[ms] - 開始時間T0[ms]
time = log.RelTick - startTime;
// 瞬間走行時間T_m[ms] = Tn[ms] - Tn-1[ms]
momentTime = time - RunTimeLog[0];
}
// 走行時間をバックアップ
RunTimeLog.Append(time);
// プロパティにセット
this.RunTime = time;
this.MomentRunTime = momentTime;
}
/// <summary>
/// 状態更新
/// </summary>
/// <param name="log">NXT Log</param>
public void UpdateState(NxtLog log)
{
this.CurrentLog = log;
UpdateTime(log);
UpdateDist(log);
UpdateSpeed();
UpdateDirection();
UpdatePosition();
UpdateTaileAngle(log);
UpdatePosture(log);
}
private void UpdateDist(NxtLog log)
{
NxtMotor <double> angle = new NxtMotor <double>();
NxtMotor <double> momentAngle = new NxtMotor <double>();
double dist = 0;
double mDist = 0;
// 1回目
if (motorRotateAngleLog.Count == 0)
{
// 開始モータカウント値をセット
motorEncoderStart = new NxtMotor <double>(log.MotorCnt1, log.MotorCnt2);
// 瞬間モータ移動距離をリセット
momentMotorDist = new NxtMotor <double>();
}
// 2回目以降
else
{
// モータ回転角度A[deg] = モータカウントCn[deg] - 開始カウント値C0[deg]
double angleR = log.MotorCnt1 - motorEncoderStart.R;
double angleL = log.MotorCnt2 - motorEncoderStart.L;
angle = new NxtMotor <double>(angleR, angleL);
// モータ移動距離MD[mm] = ( 2π * R[mm] ) * (A[deg]/360)
// R = タイヤ半径
double distR = (2 * Math.PI * TireRadius) * (angleR / 360);
double distL = (2 * Math.PI * TireRadius) * (angleL / 360);
// 移動距離D[mm] = (MD_R[mm] + MD_L[mm]) / 2
dist = (distR + distL) / 2;
// 瞬間モータ回転角度A_m[deg] = Cn[deg] - Cn-1[deg]
double mAngleR = angleR - motorRotateAngleLog[0].R;
double mAngleL = angleL - motorRotateAngleLog[0].L;
// 瞬間モータ移動距離MD_m[mm] = ( 2π * R[mm] ) * (A_m[deg]/360)
// R = タイヤ半径
double mDistR = (2 * Math.PI * TireRadius) * (mAngleR / 360);
double mDistL = (2 * Math.PI * TireRadius) * (mAngleL / 360);
this.momentMotorDist = new NxtMotor <double>(mDistR, mDistL);
// 移動距離D_m[mm] = (MD_R_m[mm] + MD_L_m[mm]) / 2
mDist = (mDistR + mDistL) / 2;
}
// モータ回転角度をバックアップ
motorRotateAngleLog.Append(angle);
// プロパティにセット
this.Distance = dist;
this.MomentDistance = mDist;
}
/// <summary>
/// データを追加
/// </summary>
/// <param name="log">NXT Log</param>
public void AppendData(NxtLog log)
{
// 全ての系列に対してデータを追加
foreach (string name in NxtLog.LogDataMember)
{
Point p = log.GetTimeSeriesData(name);
// データポイントを生成
DataPoint point = new DataPoint(p.X, p.Y);
// 系列名からデータを追加する系列を取得
Series s = logSeriesList.Find(o => o.Name == name);
// データを追加
s.Points.Add(point);
}
}
/// <summary>
/// データを追加
/// </summary>
/// <param name="log">NXT Log</param>
public void AppendData(NxtLog log)
{
// 全ての系列に対してデータを追加
foreach (string name in NxtLog.LogDataMember)
{
Point p = log.GetTimeSeriesData(name);
// データポイントを生成
DataPoint point = new DataPoint(p.X, p.Y);
// 系列名からデータを追加する系列を取得
Series s = logSeriesList.Find(o => o.Name == name);
// データを追加
s.Points.Add(point);
}
}
/// <summary>
/// Log Message Data 生成
/// </summary>
/// <param name="data"></param>
public void Append(Byte data)
{
// パケットヘッダ部取得
if (byteNo < NxtLog.PacketHeaderLen)
{
// 受信したデータをヘッダ配列へ格納
packetHeader[byteNo++] = data;
if (byteNo == NxtLog.PacketHeaderLen)
{
// パケットヘッダ解析
// パケットサイズのチェック
UInt16 len = BitConverter.ToUInt16(packetHeader, 0);
// ヘッダに格納された値がパケットデータ長と異なる
if (len != NxtLog.PacketDataLen)
{
// １byte分を読み捨てる
packetHeader[0] = packetHeader[1];
byteNo = 1;
}
}
}
// パケットデータ
else if (byteNo < NxtLog.PacketLen)
{
// 受信したデータをデータ配列へ格納
packetData[byteNo++ - NxtLog.PacketHeaderLen] = data;
if (byteNo == NxtLog.PacketLen)
{
// パケットをフィールドに変換
NxtLog log = new NxtLog(packetData);
// デリゲートを介してログデータ追加メソッドを呼び出し
this.appendMessageDelegate.Invoke(log);
// バイト番号を先頭に戻す
byteNo = 0;
}
}
else
{
// バイト番号を先頭に戻す
byteNo = 0;
}
}
/// <summary>
/// ログファイルにデータを追加
/// </summary>
/// <param name="log">ログデータ</param>
public void AppendLogFile(NxtLog log)
{
// CSV形式でログデータを取得
string rec = log.ToCsvString();
// ログファイルを開く
using (StreamWriter sw = new StreamWriter(new FileStream(this.LogFileName, FileMode.Append)))
{
try
{
// ファイルへ追記
sw.Write(rec);
}
catch (Exception ex)
{
Debug.WriteLine("FILE WRITE ERROR : {0}", ex.ToString());
}
}
}
private void UpdateTaileAngle(NxtLog log)
{
double angleDelta = 0;
double tailAngle = 0;
double tailTurnAngle = 0;
// 1回目
if (tailAngleLog.Count == 0)
{
// 尻尾初期角度[deg] = モータカウント[deg]
tailStartAngle = log.MotorCnt0;
}
else
{
// 尻尾瞬間回転角度[deg] = モータカウント[deg] - 前回尻尾角度[deg]
angleDelta = log.MotorCnt0 - tailAngleLog[0];
// 尻尾回転角度[deg] = モータカウント[deg] - 尻尾初期角度[deg]
tailTurnAngle = log.MotorCnt0 - tailStartAngle;
}
// 尻尾角度[deg] = モータカウント[deg]
tailAngle = log.MotorCnt0;
// 正の値に正規化
while (tailAngle < 0)
{
tailAngle += 360;
}
// 0～360度の範囲に正規化
tailAngle %= 360;
// 尻尾角度をバックアップ
tailAngleLog.Append(tailAngle);
// プロパティ2セット
this.TailAngle = tailAngle;
this.TailTurnAngle = tailTurnAngle;
this.MomentTailTurnAngle = angleDelta;
}
/// <summary>
/// グラフへログデータを追加
/// </summary>
private void AppendLogGraph(NxtLog logMessage)
{
// グラフにデータを追加
logChartManager.AppendData(logMessage);
}
/// <summary>
/// State Manaagerへログデータを追加
/// </summary>
private void AppendState(NxtLog logMessage)
{
// リストにログを追加
stateManager.AppendList(logMessage);
}
/// <summary>
/// Log Manaagerへログデータを追加
/// </summary>
private void AppendList(NxtLog logMessage)
{
// リストにログを追加
logManager.AppendList(logMessage);
}
/// <summary>
/// ログファイルへログデータを追加
/// </summary>
private void AppendLogFile(NxtLog logMessage)
{
// ファイルにログを追加
logManager.AppendLogFile(logMessage);
}
/// <summary>
/// ログファイルにデータを追加
/// </summary>
/// <param name="log">ログデータ</param>
public void AppendLogFile(NxtLog log)
{
// CSV形式でログデータを取得
string rec = log.ToCsvString();
// ログファイルを開く
using (StreamWriter sw = new StreamWriter(new FileStream(this.LogFileName, FileMode.Append)))
{
try
{
// ファイルへ追記
sw.Write(rec);
}
catch (Exception ex)
{
Debug.WriteLine("FILE WRITE ERROR : {0}", ex.ToString());
}
}
}
/// <summary>
/// Log Message Data 生成
/// </summary>
/// <param name="data"></param>
public void Append(Byte data)
{
// パケットヘッダ部取得
if (byteNo < NxtLog.PacketHeaderLen)
{
// 受信したデータをヘッダ配列へ格納
packetHeader[byteNo++] = data;
if (byteNo == NxtLog.PacketHeaderLen)
{
// パケットヘッダ解析
// パケットサイズのチェック
UInt16 len = BitConverter.ToUInt16(packetHeader, 0);
// ヘッダに格納された値がパケットデータ長と異なる
if (len != NxtLog.PacketDataLen)
{
// １byte分を読み捨てる
packetHeader[0] = packetHeader[1];
byteNo = 1;
}
}
}
// パケットデータ
else if (byteNo < NxtLog.PacketLen)
{
// 受信したデータをデータ配列へ格納
packetData[byteNo++ - NxtLog.PacketHeaderLen] = data;
if (byteNo == NxtLog.PacketLen)
{
// パケットをフィールドに変換
NxtLog log = new NxtLog(packetData);
// デリゲートを介してログデータ追加メソッドを呼び出し
this.appendMessageDelegate.Invoke(log);
// バイト番号を先頭に戻す
byteNo = 0;
}
}
else
{
// バイト番号を先頭に戻す
byteNo = 0;
}
}
/// <summary>
/// テキストボックスへログデータを追加
/// </summary>
private void AppendTextBox(NxtLog logMessage)
{
bufLogTextBox += logMessage.ToString();
// テキストボックスへの書込み（AppendText）を頻繁に繰り返すと
// 実行速度が低下するためバッファーを介して書込みをまとめて行う。
// ストップウォッチ停止
txtLogOutputStopwatch.Stop();
// 更新時間が一定時間内であればスキップ
if (txtLogOutputStopwatch.ElapsedMilliseconds > 20)
{
// テキストボックスへ追記
txtLogOutput.AppendText(bufLogTextBox);
// 最終行までスクロール
txtLogOutput.ScrollToEnd();
// 書込みバッファーをクリア
bufLogTextBox = String.Empty;
// ストップウォッチをリセット
txtLogOutputStopwatch.Reset();
}
// ストップウオッチを（再）スタート
txtLogOutputStopwatch.Start();
}
private void UpdatePosture(NxtLog log)
{
double av = 0;
double delta = 0;
// センサ値を取得
currentGyro = log.SensorAdc0;
// センサ値をバッファに格納
gyroBuffer.Append(currentGyro);
// 初期化中
switch (init_sate)
{
case PostureState.Init:
// 初期化中は角度0
posture = 0;
// バッファがいっぱいになった
if (gyroBuffer.Length == gyroBuffer.Count)
{
// ジャイロ平均値をオフセットに設定
offset = gyroBuffer.Average();
// 角速度を計算
av = currentGyro - offset;
anglarVelocity.Append(av);
// Ready 状態に遷移
init_sate = PostureState.Ready;
}
break;
case PostureState.Ready:
// 角速度を計算
//av = currentGyro - offset;
av = gyroBuffer.Average() - offset;
anglarVelocity.Append(av);
// Δt = 瞬間移動時間[msec] / 1000
double dt = ((double)this.MomentRunTime / 1000);
// 台形公式により、面積を計算
// ΔP =( (AVn + AVn-1) * Δt) / 2
delta = ((anglarVelocity[0] + anglarVelocity[1]) * dt) / 2;
// 不感帯を設定
if (delta <= DeadZoneThreshold && delta >= -DeadZoneThreshold)
{
delta = 0;
}
// 傾斜を更新
posture += delta;
deltaBuffer.Append(delta);
// 傾斜の更新値が一定時間ほぼ0 = 直立状態
if (-0.05 <= av && av <= 0.05)
{
posture = 0;
}
break;
}
#if DEBUG_DUMP
string rec = string.Format("{0}, {1}, {2}, {3}, {4}\r\n", currentGyro, offset, anglarVelocity[0], delta, posture);
// ログファイルを開く
using (StreamWriter sw = new StreamWriter(new FileStream("posture_dump.csv", FileMode.Append)))
{
try
{
// ファイルへ追記
sw.Write(rec);
}
catch (Exception ex)
{
Debug.WriteLine("FILE WRITE ERROR : {0}", ex.ToString());
}
}
#endif
this.Posture = posture;
}
/// <summary>
/// ログリストにデータを追加
/// </summary>
/// <param name="log">ログデータ</param>
public void AppendList(NxtLog log)
{
logList.Add(log);
logTextBuilder.AppendFormat(log.ToString());
}
/// <summary>
/// グラフへ状態データを追加
/// </summary>
private void AppendStateGraph(NxtLog logMessage)
{
// グラフにデータを追加
stateChartManager.AppendData(stateManager.CurrentState);
}
/// <summary>
/// ログファイルを開く
/// </summary>
/// <param name="filename">ファイル名</param>
/// <param name="bgWorker">Background Worker</param>
/// <param name="stateManager">Nxt State Manager</param>
public void OpenLogFile(string filename, BackgroundWorker bgWorker, NxtStateManager stateManager = null)
{
// ファイル名を保存
this.LogFileName = filename;
// CSV ファイルから行単位で読み込み
string[] file_data = File.ReadAllLines(filename);
// 行数を取得
int length = file_data.Length;
// テキストボックス出力用文字列
logTextBuilder = new StringBuilder();
logTextBuilder.AppendFormat(" Time, Data1, Data2, Battery, MotorA, MotorB, MotorC, ADC1, ADC2, ADC3, ADC4, I2C\r\n");
// CSVパーサーを生成
using (TextFieldParser parser = new TextFieldParser(filename, System.Text.Encoding.GetEncoding("Shift_JIS")))
{
// CSVは区切り形式
parser.TextFieldType = FieldType.Delimited;
// 区切り文字はコンマ
parser.SetDelimiters(",");
// 最初の1行(系列名)は捨てる
string[] comment = parser.ReadFields();
int cnt = 0;
// 最終行まで走査
while (parser.EndOfData != true)
{
// 1行分のデータを取得
string[] row = parser.ReadFields();
// CSV形式をByte配列に変換
using (MemoryStream ms = new MemoryStream())
{
// Time
ms.Write(BitConverter.GetBytes(uint.Parse(row[0])), 0, sizeof(uint));
// Data1
ms.Write(BitConverter.GetBytes(sbyte.Parse(row[1])), 0, sizeof(sbyte));
// Data2
ms.Write(BitConverter.GetBytes(sbyte.Parse(row[2])), 0, sizeof(sbyte));
// Battery
ms.Write(BitConverter.GetBytes(ushort.Parse(row[3])), 0, sizeof(ushort));
// MotorA
ms.Write(BitConverter.GetBytes(int.Parse(row[4])), 0, sizeof(int));
// MotorB
ms.Write(BitConverter.GetBytes(int.Parse(row[5])), 0, sizeof(int));
// MotorC
ms.Write(BitConverter.GetBytes(int.Parse(row[6])), 0, sizeof(int));
// ADC1
ms.Write(BitConverter.GetBytes(short.Parse(row[7])), 0, sizeof(short));
// ADC2
ms.Write(BitConverter.GetBytes(short.Parse(row[8])), 0, sizeof(short));
// ADC3
ms.Write(BitConverter.GetBytes(short.Parse(row[9])), 0, sizeof(short));
// ADC4
ms.Write(BitConverter.GetBytes(short.Parse(row[10])), 0, sizeof(short));
// I2C
ms.Write(BitConverter.GetBytes(int.Parse(row[11])), 0, sizeof(int));
// ログを生成
NxtLog log = new NxtLog(ms.ToArray());
// リストに追加
logList.Add(log);
// テキストボックス出力用文字列にログを追加
logTextBuilder.AppendFormat(log.ToString());
// Nxt State Managerが初期化済みかつ、NXT State読み込み設定がON
if (stateManager != null && Properties.Settings.Default.LoadNxtStateFromLogFile == true)
{
// 状態を更新
stateManager.AppendList(log);
}
}
cnt++;
// 進捗率を計算
double parcent = ((double)cnt / (double)length) * 100;
// 進捗率を通知
bgWorker.ReportProgress((int)parcent);
}
}
}
/// <summary>
/// ログファイルへログデータを追加
/// </summary>
private void AppendLogFile(NxtLog logMessage)
{
// ファイルにログを追加
logManager.AppendLogFile(logMessage);
}
/// <summary>
/// Log Manaagerへログデータを追加
/// </summary>
private void AppendList(NxtLog logMessage)
{
// リストにログを追加
logManager.AppendList(logMessage);
}
/// <summary>
/// グラフへ状態データを追加
/// </summary>
private void AppendStateGraph(NxtLog logMessage)
{
// グラフにデータを追加
stateChartManager.AppendData(stateManager.CurrentState);
}
/// <summary>
/// 状態リストにデータを追加
/// </summary>
/// <param name="log">ログデータ</param>
public void AppendList(NxtLog log)
{
state.UpdateState(log);
stateLogList.Add((NxtState)state.Clone());
stateTextBuilder.AppendFormat(state.ToString());
}
/// <summary>
/// グラフへログデータを追加
/// </summary>
private void AppendLogGraph(NxtLog logMessage)
{
// グラフにデータを追加
logChartManager.AppendData(logMessage);
}
/// <summary>
/// ログファイルを開く
/// </summary>
/// <param name="filename">ファイル名</param>
/// <param name="bgWorker">Background Worker</param>
/// <param name="stateManager">Nxt State Manager</param>
public void OpenLogFile(string filename, BackgroundWorker bgWorker, NxtStateManager stateManager = null)
{
// ファイル名を保存
this.LogFileName = filename;
// CSV ファイルから行単位で読み込み
string[] file_data = File.ReadAllLines(filename);
// 行数を取得
int length = file_data.Length;
// テキストボックス出力用文字列
logTextBuilder = new StringBuilder();
logTextBuilder.AppendFormat(" Time, Data1, Data2, Battery, MotorA, MotorB, MotorC, ADC1, ADC2, ADC3, ADC4, I2C\r\n");
// CSVパーサーを生成
using (TextFieldParser parser = new TextFieldParser(filename, System.Text.Encoding.GetEncoding("Shift_JIS")))
{
// CSVは区切り形式
parser.TextFieldType = FieldType.Delimited;
// 区切り文字はコンマ
parser.SetDelimiters(",");
// 最初の1行(系列名)は捨てる
string[] comment = parser.ReadFields();
int cnt = 0;
// 最終行まで走査
while (parser.EndOfData != true)
{
// 1行分のデータを取得
string[] row = parser.ReadFields();
// CSV形式をByte配列に変換
using (MemoryStream ms = new MemoryStream())
{
// Time
ms.Write(BitConverter.GetBytes(uint.Parse(row[0])), 0, sizeof(uint));
// Data1
ms.Write(BitConverter.GetBytes(sbyte.Parse(row[1])), 0, sizeof(sbyte));
// Data2
ms.Write(BitConverter.GetBytes(sbyte.Parse(row[2])), 0, sizeof(sbyte));
// Battery
ms.Write(BitConverter.GetBytes(ushort.Parse(row[3])), 0, sizeof(ushort));
// MotorA
ms.Write(BitConverter.GetBytes(int.Parse(row[4])), 0, sizeof(int));
// MotorB
ms.Write(BitConverter.GetBytes(int.Parse(row[5])), 0, sizeof(int));
// MotorC
ms.Write(BitConverter.GetBytes(int.Parse(row[6])), 0, sizeof(int));
// ADC1
ms.Write(BitConverter.GetBytes(short.Parse(row[7])), 0, sizeof(short));
// ADC2
ms.Write(BitConverter.GetBytes(short.Parse(row[8])), 0, sizeof(short));
// ADC3
ms.Write(BitConverter.GetBytes(short.Parse(row[9])), 0, sizeof(short));
// ADC4
ms.Write(BitConverter.GetBytes(short.Parse(row[10])), 0, sizeof(short));
// I2C
ms.Write(BitConverter.GetBytes(int.Parse(row[11])), 0, sizeof(int));
// ログを生成
NxtLog log = new NxtLog(ms.ToArray());
// リストに追加
logList.Add(log);
// テキストボックス出力用文字列にログを追加
logTextBuilder.AppendFormat(log.ToString());
// Nxt State Managerが初期化済みかつ、NXT State読み込み設定がON
if (stateManager != null && Properties.Settings.Default.LoadNxtStateFromLogFile == true)
{
// 状態を更新
stateManager.AppendList(log);
}
}
cnt++;
// 進捗率を計算
double parcent = ((double)cnt / (double)length) * 100;
// 進捗率を通知
bgWorker.ReportProgress((int)parcent);
}
}
}
/// <summary>
/// ログリストにデータを追加
/// </summary>
/// <param name="log">ログデータ</param>
public void AppendList(NxtLog log)
{
logList.Add(log);
logTextBuilder.AppendFormat(log.ToString());
}
/// <summary>
/// State Manaagerへログデータを追加
/// </summary>
private void AppendState(NxtLog logMessage)
{
// リストにログを追加
stateManager.AppendList(logMessage);
}
