refactor: クラス名および変数を変更した

module/API/Controller.cpp

@@ -8,9 +8,9 @@
 Controller::Controller() : rightWheel(PORT_B), leftWheel(PORT_C), armMotor(PORT_A) {}
 
 // PWMの初期化
-double Controller::manageRightPwm = 0.0;
-double Controller::manageLeftPwm = 0.0;
-double Controller::manageArmPwm = 0.0;
+double Controller::pwmOfRightWheel = 0.0;
+double Controller::pwmOfLeftWheel = 0.0;
+double Controller::pwmOfArm = 0.0;
 
 // モータに設定するPWM値の制限
 int Controller::limitPwmValue(const int value)
@@ -26,66 +26,69 @@ int Controller::limitPwmValue(const int value)
 // 右モータにPWM値をセット
 void Controller::setRightMotorPwm(const int pwm)
 {
-  manageRightPwm = pwm;
+  pwmOfRightWheel = pwm;
   rightWheel.setPWM(limitPwmValue(pwm));
 }
 
 // 左モータにPWM値をセット
 void Controller::setLeftMotorPwm(const int pwm)
 {
-  manageLeftPwm = pwm;
+  pwmOfLeftWheel = pwm;
   leftWheel.setPWM(limitPwmValue(pwm));
 }
 
 // 右モータのPWM値をリセット
 void Controller::resetRightMotorPwm()
 {
-  manageRightPwm = 0;
+  pwmOfRightWheel = 0.0;
   rightWheel.reset();
 }
 
 // 左モータのPWM値をリセット
 void Controller::resetLeftMotorPwm()
 {
-  manageLeftPwm = 0;
+  pwmOfLeftWheel = 0.0;
   leftWheel.reset();
 }
 
 // タイヤのモータを停止する
 void Controller::stopMotor()
 {
-  manageRightPwm = 0.0;
-  manageLeftPwm = 0.0;
+  pwmOfRightWheel = 0.0;
+  pwmOfLeftWheel = 0.0;
   leftWheel.stop();
   rightWheel.stop();
 }
 
 // アームのモータにPWM値をセット
 void Controller::setArmMotorPwm(const int pwm)
 {
+  pwmOfArm = pwm;
   armMotor.setPWM(limitPwmValue(pwm));
 }
 
 // アームのモータのPWM値をリセット
 void Controller::resetArmMotorPwm()
 {
+  pwmOfArm = 0;
   armMotor.reset();
 }
 
 // アームのモータを停止する
 void Controller::stopArmMotor()
 {
+  pwmOfArm = 0;
   armMotor.stop();
 }
 
 // 右タイヤのPWMを取得する
 double Controller::getRightPwm()
 {
-  return manageRightPwm;
+  return pwmOfRightWheel;
 }
 
 // 左タイヤのPWMを取得する
 double Controller::getLeftPwm()
 {
-  return manageLeftPwm;
+  return pwmOfLeftWheel;
 }

module/API/Controller.h

@@ -72,9 +72,9 @@ class Controller {
   ev3api::Motor rightWheel;
   ev3api::Motor leftWheel;
   ev3api::Motor armMotor;
-  static double manageRightPwm;  // 右タイヤPWM
-  static double manageLeftPwm;   // 左タイヤPWM
-  static double manageArmPwm;    // アームPWM
+  static double pwmOfRightWheel;  // 右タイヤPWM
+  static double pwmOfLeftWheel;   // 左タイヤPWM
+  static double pwmOfArm;         // アームPWM
 
   /**
    * @brief モータに設定するPWM値の制限

module/Calculator/Speed.cpp

@@ -0,0 +1,91 @@
+/**
+ *  @file Speed.cpp
+ *  @brief 目標速度に対応するPWM値を算出するクラス
+ *  @author CHIHAYATAKU
+ */
+#include "Speed.h"
+
+Speed::Speed(double _targetSpeed)
+  : rightTargetSpeed(_targetSpeed),
+    leftTargetSpeed(_targetSpeed),
+    rightPid(K_P, K_I, K_D, _targetSpeed),
+    leftPid(K_P, K_I, K_D, _targetSpeed)
+{
+  rightPwm = Controller::getRightPwm();
+  leftPwm = Controller::getLeftPwm();
+  int rightAngle = measurer.getRightCount();
+  int leftAngle = measurer.getLeftCount();
+  prevRightMileage = Mileage::calculateWheelMileage(rightAngle);
+  prevLeftMileage = Mileage::calculateWheelMileage(leftAngle);
+  int currentTime = timer.now();
+  prevRightTime = currentTime;
+  prevLeftTime = currentTime;
+}
+
+Speed::Speed(double _rightTargetSpeed, double _leftTargetSpeed)
+  : rightTargetSpeed(_rightTargetSpeed),
+    leftTargetSpeed(_leftTargetSpeed),
+    rightPid(R_K_P, R_K_I, R_K_D, _rightTargetSpeed),
+    leftPid(R_K_P, R_K_I, R_K_D, _leftTargetSpeed)
+{
+  rightPwm = Controller::getRightPwm();
+  leftPwm = Controller::getLeftPwm();
+  int rightAngle = measurer.getRightCount();
+  int leftAngle = measurer.getLeftCount();
+  prevRightMileage = Mileage::calculateWheelMileage(rightAngle);
+  prevLeftMileage = Mileage::calculateWheelMileage(leftAngle);
+  int currentTime = timer.now();
+  prevRightTime = currentTime;
+  prevLeftTime = currentTime;
+}
+
+double Speed::calcRightPwmFromSpeed()
+{
+  // 右タイヤの回転角度を取得
+  int rightAngle = measurer.getRightCount();
+  // 右タイヤの走行距離を算出
+  double currentRightMileage = Mileage::calculateWheelMileage(rightAngle);
+  double diffRightMileage = currentRightMileage - prevRightMileage;
+  // 走行時間を算出
+  int currentRightTime = timer.now();
+  double diffRightTime = (double)(currentRightTime - prevRightTime);
+  // 右タイヤの走行速度を算出
+  double currentRightSpeed = calcSpeed(diffRightMileage, diffRightTime);
+  // 走行速度に相当する右タイヤのPWM値を算出
+  rightPwm += rightPid.calculatePid(currentRightSpeed, diffRightTime);
+  // メンバを更新
+  prevRightMileage = currentRightMileage;
+  prevRightTime = currentRightTime;
+
+  return rightPwm;
+}
+
+double Speed::calcLeftPwmFromSpeed()
+{
+  // 左タイヤの回転角度を取得
+  int leftAngle = measurer.getLeftCount();
+  // 左タイヤの走行距離を算出
+  double currentLeftMileage = Mileage::calculateWheelMileage(leftAngle);
+  double diffLeftMileage = currentLeftMileage - prevLeftMileage;
+  // 走行時間を算出
+  int currentLeftTime = timer.now();
+  double diffLeftTime = (double)(currentLeftTime - prevLeftTime);
+  // 左タイヤの走行速度を算出
+  double currentLeftSpeed = calcSpeed(diffLeftMileage, diffLeftTime);
+  // 走行速度に相当する左タイヤのPWM値を算出
+  leftPwm += leftPid.calculatePid(currentLeftSpeed, diffLeftTime);
+  // メンバを更新
+  prevLeftMileage = currentLeftMileage;
+  prevLeftTime = currentLeftTime;
+
+  return leftPwm;
+}
+
+double Speed::calcSpeed(double diffMileage, double diffTime)
+{
+  // 走行時間が0のとき、0を返す
+  if(diffTime == 0.0) return 0.0;
+  // 走行速度[mm/s]を算出(= 1000 * mm / ms)
+  double speed = 1000 * diffMileage / diffTime;
+  return speed;
+}

module/Calculator/Speed.h

@@ -0,0 +1,74 @@
+/**
+ * @file Speed.h
+ *  @brief 目標速度に対応するPWM値を算出するクラス
+ *  @author CHIHAYATAKU
+ */
+
+#ifndef Speed_H
+#define Speed_H
+
+#include "Mileage.h"
+#include "Pid.h"
+#include "Timer.h"
+#include "Controller.h"
+#include "Measurer.h"
+
+class Speed {
+ public:
+  /**
+   * @brief 引数１つの場合のコンストラクタ
+   * @param _targetSpeed 目標とする走行速度[mm/s]
+   */
+  Speed(double _targetSpeed);
+
+  /**
+   * @brief 引数２つの場合のコンストラクタ
+   * @param _rightTargetSpeed 目標とする右タイヤ走行速度[mm/s]
+   * @param _leftTargetSpeed 目標とする左タイヤ走行速度[mm/s]
+   */
+  Speed(double _rightTargetSpeed, double _leftTargetSpeed);
+
+  /**
+   * @brief 目標とする走行速度に相当する右車輪のPWM値を算出する
+   * @return 走行速度に相当する右タイヤのPWM値
+   */
+  double calcRightPwmFromSpeed();
+
+  /**
+   * @brief 目標とする走行速度に相当する左車輪のPWM値を算出する
+   * @return 走行速度に相当する左タイヤのPWM値
+   */
+  double calcLeftPwmFromSpeed();
+
+ private:
+  const double rightTargetSpeed;
+  const double leftTargetSpeed;
+  Pid rightPid;
+  Pid leftPid;
+  Timer timer;
+  Measurer measurer;
+  double rightPwm;
+  double leftPwm;
+  double prevRightMileage;
+  double prevLeftMileage;
+  int prevRightTime;
+  int prevLeftTime;
+  // 回頭以外のPIDゲイン
+  static constexpr double K_P = 0.004;
+  static constexpr double K_I = 0.0000005;
+  static constexpr double K_D = 0.0007;
+  // 回頭用PIDゲイン
+  // ToDo：次イテレーションで調整
+  static constexpr double R_K_P = 0.004;
+  static constexpr double R_K_I = 0.0000005;
+  static constexpr double R_K_D = 0.0007;
+
+  /**
+   * @brief 走行速度を算出する
+   * @param diffMileage 移動距離[mm]
+   * @param diffTime 移動時間[ms]
+   * @return 走行速度[mm/s]
+   */
+  double calcSpeed(double diffMileage, double diffTime);
+};
+#endif