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;
}
private void UpdateDirection()
{
double direction = startDirection;
// モータ回転角度を取得
NxtMotor <double> rotateAngle = motorRotateAngleLog[0];
// 方向 = 開始方向 + ((タイヤ半径[mm] / 車軸長[mm]) * (右モータ回転角度[deg] - 左モータ回転角度[deg]))
direction += (TireRadius / AxleLen) * (rotateAngle.R - rotateAngle.L);
// 方向がマイナスの間
while (direction < 0)
{
// 方向を正に補正
direction += 360;
}
// 方向を0～360[deg]で正規化
direction %= 360;
// プロパティをセット
this.Direction = direction;
}