/// <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) { // 1byte分を読み捨てる 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) { // 1byte分を読み捨てる 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); }