/// <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);
}