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