/// <summary>
/// ロボットを動かすための処理．
/// </summary>
/// <param name="sender"></param>
/// <param name="e"></param>
private void startButton_Click(object sender, EventArgs e)
{
RobotControlDelegate contRobo = new RobotControlDelegate(delegate { });
m1 = new Motor(dt);
m2 = new Motor(dt);
m3 = new Motor(dt);
m4 = new Motor(dt);
robo = new OmniRobot(dt);
try
{
udp = new UdpClient(port);
writeLog("Ethernet Start");
remoteEP = new IPEndPoint(IPAddress.Any, port);
//非同期受信の開始
//udp.BeginReceive(ReceiveCallback, udp);
}
catch (SocketException ex)
{
writeLog("Ethernet Error! : " + ex.ToString());
}
//初期カウント数を得るためとロボットと通信できるか確認するための処理
pwmDataSend(0, 0, 0, 0);
if (waitDataReceive(500) == false)
{
udp.Close();
writeLog("ロボットとの通信ができませんでした．");
return;
}
//motorの初期化処理
int temp;
temp = m1.Count;
m1.Init();
m1.offsetCnt = temp;
temp = m2.Count;
m2.Init();
m2.offsetCnt = temp;
temp = m3.Count;
m3.Init();
m3.offsetCnt = temp;
temp = m4.Count;
m4.Init();
m4.offsetCnt = temp;
timer1.Enabled = true;
writeLog("走行開始");
//受信データ保存用に初期化
mpuData1.Clear();
mpuData2.Clear();
timestamp.Clear();
//それぞれの動作モードでの処理を記述
if (pwmControl.Checked == true)
{
int m1pwm = (int)pwmM1.Value;
int m2pwm = (int)pwmM2.Value;
int m3pwm = (int)pwmM3.Value;
int m4pwm = (int)pwmM4.Value;
int rtime = (int)controlTimeset.Value;
contRobo = new RobotControlDelegate(delegate
{
int i = 1;
sw.Start();
pwmDataSend(0, 0, 0, 0);
while (sw.ElapsedMilliseconds < rtime)
{
if (udpReceiveCheck(udp))
{
udpDataReceive(udp, remoteEP);
}
if (sw.ElapsedMilliseconds > i * dt * 1000 && dataflag1+dataflag2 == 2)
{
i++;
dataflag1 = 0;
dataflag2 = 0;
robo.update(m1, m2, m3, m4);
pwmDataSend(m1pwm, m2pwm, m3pwm, m4pwm);
}
}
});
}
if(pidControl.Checked == true)
{
int rtime = (int)controlTimeset.Value;
double kp = (double)pGainSet.Value;
double ki = (double)iGainSet.Value;
double kd = (double)dGainSet.Value;
double targetVxc = (double)vxcSet.Value;
double targetVyc = (double)vycSet.Value;
double targetAvc = (double)avcSet.Value;
int m1pwm, m2pwm, m3pwm, m4pwm;
double tv1 = 0, tv2 = 0, tv3 = 0, tv4 = 0;
contRobo = new RobotControlDelegate(delegate
{
int i = 1;
sw.Start();
pwmDataSend(0, 0, 0, 0);
while (sw.ElapsedMilliseconds < rtime)
{
if (udpReceiveCheck(udp))
{
udpDataReceive(udp, remoteEP);
}
if (sw.ElapsedMilliseconds > i * dt * 1000)
{
//データが受信できているかチェックして，できていなければ補間する
if(dataflag1 == 0)
{
timestamp.Append("#d1,").Append(m1.Count.ToString()).Append(",").AppendLine(m4.Count.ToString());
m1.update(m1.PWM, (2 * m1.Count - m1.OldCount + m1.offsetCnt));
m4.update(m4.PWM, (2 * m4.Count - m4.OldCount + m4.offsetCnt));
interdataflag1++;
interpolationNum++;
timestamp.Append("#d1,").Append(m1.Count.ToString()).Append(",").AppendLine(m4.Count.ToString());
}
if (dataflag2 == 0)
{
timestamp.Append("#d2,").Append(m3.Count.ToString()).Append(",").AppendLine(m2.Count.ToString());
m3.update(m3.PWM, (2 * m3.Count - m3.OldCount + m3.offsetCnt));
m2.update(m2.PWM, (2 * m2.Count - m2.OldCount + m2.offsetCnt));
interdataflag2++;
interpolationNum++;
timestamp.Append("#d2,").Append(m3.Count.ToString()).Append(",").AppendLine(m2.Count.ToString());
}
i++;
dataflag1 = 0;
dataflag2 = 0;
robo.update(m1, m2, m3, m4);
robo.getMotorSpped(targetVxc, targetVyc, targetAvc, ref tv1, ref tv2, ref tv3, ref tv4);
m1pwm = m1.pid(kp, ki, kd, tv1);
m2pwm = m2.pid(kp, ki, kd, tv2);
m3pwm = m3.pid(kp, ki, kd, tv3);
m4pwm = m4.pid(kp, ki, kd, tv4);
pwmDataSend(m1pwm, m2pwm, m3pwm, m4pwm);
timestamp.Append(sw.ElapsedMilliseconds.ToString()).AppendLine(",send");
}
}
});
}
//動作が終了したときに呼ばれる関数を設定
AsyncCallback callback = new AsyncCallback(endControl);
IAsyncResult ar = contRobo.BeginInvoke(callback, null);
}
/// <summary>
/// データファイルを解析して使いやすくする
/// </summary>
/// <param name="datapath1">M1, M3のデータが記述されたファイル</param>
/// <param name="datapath2">M2, M4のデータが記述されたファイル</param>
/// <param name="filename">保存するファイル名</param>
private void dataFileAnalyze(string datapath1, string datapath2, string filename, string comment="")
{
//csvファイル殻読み込んだデータを使いやすくするためのリスト
var data1pre = new List<List<string>>();
var data2pre = new List<List<string>>();
var data1 = new List<List<string>>();
var data2 = new List<List<string>>();
//時系列でロボットの状態を記録していくリスト
List<List<string>> wdata = new List<List<string>>();
//ロボットの状態を初期化
m1 = new Motor(dt);
m2 = new Motor(dt);
m3 = new Motor(dt);
m4 = new Motor(dt);
robo = new OmniRobot(dt);
//csvファイルを読み込んでlistに入れる
using (var csv = new CsvReader(datapath1))
{
data1pre = csv.ReadToEnd();
}
using (var csv = new CsvReader(datapath2))
{
data2pre = csv.ReadToEnd();
}
//必要なデータを抜き出す
foreach (var row in data1pre)
{
//まれに時刻データが抜けてるときがあるっぽいので簡易チェック
//コメント行(先頭が#)もこれではじける
if (char.IsNumber(row[0], 0))
{
var record = new List<string>();
record.Add(row[0]);
record.Add(row[2]);
record.Add(row[3]);
record.Add(row[6]);
record.Add(row[7]);
data1.Add(record);
}
}
foreach (var row in data2pre)
{
if (char.IsNumber(row[0], 0))
{
var record = new List<string>();
record.Add(row[0]);
record.Add(row[2]);
record.Add(row[3]);
record.Add(row[6]);
record.Add(row[7]);
data2.Add(record);
}
}
//エンコーダの積算値を記録開始時のものを基準にする
//これは複数回動かしていると最初に帰ってくるエンコーダの値が0でなくなるため
m1.offsetCnt = int.Parse(data1[0][3]);
m2.offsetCnt = int.Parse(data2[0][4]);
m3.offsetCnt = int.Parse(data2[0][3]);
m4.offsetCnt = int.Parse(data1[0][4]);
//要素数の少ないほうを基準にする(最後のほうのデータを受け取る前に終了している可能性があるので)
for (var i = 0; i < Math.Min(data1.Count, data2.Count); i++)
{
double time = i * dt;
//ロボットの状態を更新
m1.update(int.Parse(data1[i][1]), int.Parse(data1[i][3]));
m2.update(int.Parse(data2[i][2]), int.Parse(data2[i][4]));
m3.update(int.Parse(data2[i][1]), int.Parse(data2[i][3]));
m4.update(int.Parse(data1[i][2]), int.Parse(data1[i][4]));
robo.update(m1, m2, m3, m4);
//出力するデータの用意
var record = new List<string>();
//record.Add(((int.Parse(data1[i][0]) + int.Parse(data2[i][0])) / 2).ToString());
record.Add(time.ToString());
record.Add(m1.PWM.ToString());
record.Add(m2.PWM.ToString());
record.Add(m3.PWM.ToString());
record.Add(m4.PWM.ToString());
record.Add(m1.Count.ToString());
record.Add(m2.Count.ToString());
record.Add(m3.Count.ToString());
record.Add(m4.Count.ToString());
record.Add(m1.Velocity.ToString());
record.Add(m2.Velocity.ToString());
record.Add(m3.Velocity.ToString());
record.Add(m4.Velocity.ToString());
record.Add(m1.Acceleration.ToString());
record.Add(m2.Acceleration.ToString());
record.Add(m3.Acceleration.ToString());
record.Add(m4.Acceleration.ToString());
record.Add(m1.Angle.ToString());
record.Add(m2.Angle.ToString());
record.Add(m3.Angle.ToString());
record.Add(m4.Angle.ToString());
record.Add(robo.VelocityX.ToString());
record.Add(robo.VelocityY.ToString());
record.Add(robo.AnglerVelocity.ToString());
record.Add(robo.X.ToString());
record.Add(robo.Y.ToString());
record.Add(robo.Angle.ToString());
wdata.Add(record);
}
//結果の出力
//出力用のフォルダがない場合は生成
if (!Directory.Exists(dataFolder + "\\processed"))
{
Directory.CreateDirectory(dataFolder + "\\processed");
}
using (var writer = new CsvWriter(dataFolder + "\\processed\\" + filename + ".csv"))
{
var outData = new List<List<string>>();
//1行目にコメントを書き込み
var com = new List<string>();
com.Add("#");
com.AddRange(comment.Split(','));
outData.Add(com);
//2行目にデータ名を書き込み
outData.Add(new List<string> { "Time[s]", "PWM M1", "PWM M2", "PWM M3", "PWM M4",
"Count M1", "Count M2", "Count M3", "Count M4",
"Velocity M1 [m/s]", "Velocity M2", "Velocity M3", "Velocity M4",
"Acceleration M1 [m/s/s]", "Acceleration M2", "Acceleration M3", "Acceleration M4",
"Angle M1 [rad]", "Angle M2", "Angle M3", "Angle M4",
"Velocity X [m/s]", "Velocity Y [m/s]", "Angler Velocity[rad/s]",
"Position X [m]", "Position Y [m]", "Angle[rad]"
});
outData.AddRange(wdata);
writer.Write(outData);
}
writeLog("ファイルの書き込み完了 : " + dataFolder + "\\processed\\" + filename + ".csv");
}
