Add CubeWheel emulation. (#74)

* Add Files.
Exclude cubewheel.cpp to avoid Build Error.

* modify indent and character code

* change format

* change format

* change format

* change format

* rename filename to devide cube and wheel

* rename text in file to devide cube and wheel

CMakeLists.txt

@@ -78,7 +78,7 @@ add_subdirectory(${S2E_CORE_DIR}/src/simulation s2e_core/simulation)
 # Add all cpp files as SOURCE_FILES
 file(GLOB_RECURSE SOURCE_FILES ${CMAKE_CURRENT_LIST_DIR}/src/*.cpp)
 # Uncomment the following line to exclude any files that match the REGEX from SOURCE_FILES
-# list(FILTER SOURCE_FILES EXCLUDE REGEX ${CMAKE_CURRENT_LIST_DIR}/src/Example/example.cpp)
+list(FILTER SOURCE_FILES EXCLUDE REGEX ${CMAKE_CURRENT_LIST_DIR}/src/component/aocs/cube_wheel.cpp)
 
 # Create executable file
 add_executable(${PROJECT_NAME} ${SOURCE_FILES})

data/initialize_files/components/cubewheelsmall.ini

@@ -0,0 +1,137 @@
+[RW1]
+// prescaler defines update period of thie component
+// update period = prescaler * step sec
+prescaler = 1
+//Inertia of the RW [kgm^2]
+inertia = 2.11e-6
+//Maximum torque [Nm]
+max_torque = 0.00023
+//Maximum angular velocity [rpm]
+//The RW can drive (-max_angular_velocity,+max_angular_velocity) 
+max_angular_velocity = 8000.0
+// Direction of axis of the RW at the body fixied frame
+// plus means direction of rotation (output torque is minus direction)
+direction_b(0) = -1.0
+direction_b(1) = 0.0
+direction_b(2) = 0.0
+//The control delay[s]
+dead_time = 1.0
+//Coefficient of first order lag for driving case
+firstorder_lag_coef(0) = 2.0
+firstorder_lag_coef(1) = 0.0
+firstorder_lag_coef(2) = 0.0
+//Coefficient of first order lag for coasting case
+coasting_lag_coef(0) = 2.0
+coasting_lag_coef(1) = 0.0
+coasting_lag_coef(2) = 0.0
+//For drive initialize
+//They should be zero for normal case
+motor_drive_init = 0
+angular_velocity_init = 0.0
+
+//Parameters for calculate RW jitter
+jitter_calculation = DISABLE
+jitter_logging = DISABLE
+radial_force_harmonics_coef_path = ../../data/ini/components/RWDisturbance/radial_force_harmonics_coef.csv
+radial_torque_harmonics_coef_path = ../../data/ini/components/RWDisturbance/radial_torque_harmonics_coef.csv
+harmonics_degree = 12
+considers_structural_resonance = DISABLE
+structural_resonance_freq = 585.0 //[Hz]
+damping_factor = 0.1 //[ ]
+bandwidth = 0.001 //[ ]
+
+// Power Port
+minimum_voltage = 8.8 // V
+assumed_power_consumption = 0.88 //W
+
+[RW2]
+// prescaler defines update period of thie component
+// update period = prescaler * step sec
+prescaler = 1
+//Inertia of the RW [kgm^2]
+inertia = 2.11e-6
+//Maximum torque [Nm]
+max_torque = 0.00023
+//Maximum angular velocity [rpm]
+//The RW can drive (-max_angular_velocity,+max_angular_velocity) 
+max_angular_velocity = 8000.0
+// Direction of axis of the RW at the body fixied frame
+// plus means direction of rotation (output torque is minus direction)
+direction_b(0) = 0.0
+direction_b(1) = -1.0
+direction_b(2) = 0.0
+//The control delay[s]
+dead_time = 1.0
+//Constant of first order lag for driving case
+firstorder_lag_coef(0) = 2.0
+firstorder_lag_coef(1) = 0.0
+firstorder_lag_coef(2) = 0.0
+//Constant of first order lag for coasting case
+coasting_lag_coef(0) = 2.0
+coasting_lag_coef(1) = 0.0
+coasting_lag_coef(2) = 0.0
+//For drive initialize
+//They should be zero for normal case
+motor_drive_init = 0
+angular_velocity_init = 0.0
+
+//Parameters for calculate RW jitter
+jitter_calculation = DISABLE
+jitter_logging = DISABLE
+radial_force_harmonics_coef_path = ../../data/ini/components/RWDisturbance/radial_force_harmonics_coef.csv
+radial_torque_harmonics_coef_path = ../../data/ini/components/RWDisturbance/radial_torque_harmonics_coef.csv
+harmonics_degree = 12
+considers_structural_resonance = DISABLE
+structural_resonance_freq = 585.0 //[Hz]
+damping_factor = 0.1 //[ ]
+bandwidth = 0.001 //[ ]
+
+// Power Port
+minimum_voltage = 8.8 // V
+assumed_power_consumption = 0.88 //W
+
+[RW3]
+// prescaler defines update period of thie component
+// update period = prescaler * step sec
+prescaler = 1
+//Inertia of the RW [kgm^2]
+inertia = 2.11e-6
+//Maximum torque [Nm]
+max_torque = 0.00023
+//Maximum angular velocity [rpm]
+//The RW can drive (-max_angular_velocity,+max_angular_velocity) 
+max_angular_velocity = 8000.0
+// Direction of axis of the RW at the body fixied frame
+// plus means direction of rotation (output torque is minus direction)
+direction_b(0) = 0.0
+direction_b(1) = 0.0
+direction_b(2) = -1.0
+//The control deray[s]
+dead_time = 1.0
+//Constant of first order lag for driving case
+firstorder_lag_coef(0) = 2.0
+firstorder_lag_coef(1) = 0.0
+firstorder_lag_coef(2) = 0.0
+//Constant of first order lag for coasting case
+coasting_lag_coef(0) = 2.0
+coasting_lag_coef(1) = 0.0
+coasting_lag_coef(2) = 0.0
+//For drive initialize
+//They should be zero for normal case
+motor_drive_init = 0
+angular_velocity_init = 0.0
+
+//Parameters for calculate RW jitter
+jitter_calculation = DISABLE
+jitter_logging = DISABLE
+radial_force_harmonics_coef_path = ../../data/ini/components/RWDisturbance/radial_force_harmonics_coef.csv
+radial_torque_harmonics_coef_path = ../../data/ini/components/RWDisturbance/radial_torque_harmonics_coef.csv
+harmonics_degree = 12
+considers_structural_resonance = DISABLE
+structural_resonance_freq = 585.0 //[Hz]
+damping_factor = 0.1 //[ ]
+bandwidth = 0.001 //[ ]
+
+// Power Port
+minimum_voltage = 8.8 // V
+assumed_power_consumption = 0.88 //W

src/component/aocs/cube_wheel.cpp

@@ -0,0 +1,138 @@
+#include "cube_wheel.h"
+
+CubeWheel::CubeWheel(RWModel rw_model, const int port_id, const unsigned char i2c_addr, OBC* obc)
+    : RWModel(rw_model), ObcI2cCommunicationBase(port_id, i2c_addr, obc) {
+  port_id_ = i2c_addr - 0x67;  // port_idは1固定のため。
+
+  unsigned char data[] = {0x08};
+
+  if (i2c_addr == 0x68) {
+    WriteRegister(0x80, data, 1);
+    data[0] = 0xd0;
+    WriteRegister(0x83, data, 1);
+  } else if (i2c_addr == 0x69) {
+    WriteRegister(0x80, data, 1);
+    data[0] = 0xd2;
+    WriteRegister(0x83, data, 1);
+  } else if (i2c_addr == 0x6A) {
+    WriteRegister(0x80, data, 1);
+    data[0] = 0xd4;
+    WriteRegister(0x83, data, 1);
+  }
+}
+
+void CubeWheel::MainRoutine(int count) {
+  // Control Modeの取得と処理
+  ReadRegister(kWriteCmdControlMode_, &control_mode_, 1);
+
+  if (control_mode_ == 2) {  // Duty cycle input mode
+    SetDriveFlag(true);
+
+    // 指令dutyを取得。
+    unsigned char dutyH = 0;
+    unsigned char dutyL = 0;
+    ReadRegister(0x03, &dutyH, 1);  // 実際はアドレス3に2byte書かれるが、SILS上はC2A側でアドレス3,4に書く。
+    ReadRegister(0x04, &dutyL, 1);
+    double duty = (double)((int16_t)((uint16_t)dutyH + ((uint16_t)dutyL << 8)));
+
+    // 指令トルクをセット。
+    double torque_Nm = 0.00023 * duty / 100.0;
+
+    SetTargetTorqueRw(torque_Nm);
+  } else if (control_mode_ == 3) {  // Speed controller mode
+    SetDriveFlag(true);
+
+    // 指令speedを取得。
+    unsigned char speedH = 0;
+    unsigned char speedL = 0;
+    ReadRegister(0x05, &speedH, 1);  // 実際はアドレス2に2byte書かれるが、SILS上はC2A側でアドレス5,6に書く。
+    ReadRegister(0x06, &speedL, 1);
+
+    if (speedH != 0xff && speedL != 0xff) {
+      double speed_raw = (double)((int16_t)((uint16_t)speedH + ((uint16_t)speedL << 8)));
+      double commanded_speed_rpm = speed_raw / 2;
+      double current_speed_rpm = GetVelocityRpm();
+      double to_add_rpm = abs(commanded_speed_rpm - current_speed_rpm) / 2;
+      double to_add_rad_s = to_add_rpm * 0.1047;
+      double sign = 1;
+      if (commanded_speed_rpm < current_speed_rpm) {
+        sign = -1;
+      }
+      double speed_cmd_cycle_in_sec = 0.5;
+      double target_omega_dash = to_add_rad_s / speed_cmd_cycle_in_sec * sign;
+
+      // 指令トルクをセット。
+      double torque_Nm = target_omega_dash * 0.00000211;
+      SetTargetTorqueRw(torque_Nm);
+
+      // dummy値を書き込む。次にspeed cmdを受信するまで処理をしないため。
+      unsigned char dummy[] = {0xff, 0xff};
+      WriteRegister(0x05, dummy, 2);
+      ReadRegister(0x05, &speedH, 1);  // 実際はアドレス2に2byte書かれるが、SILS上はC2A側でアドレス5,6に書く。
+      ReadRegister(0x06, &speedL, 1);
+    }
+  }
+
+  // Backup Modeの取得と処理。
+  ReadRegister(kWriteCmdBackupMode_, &backup_mode_state_, 1);
+
+  GenerateTelemetry();
+
+  CalcTorque();
+
+  return;
+}
+
+std::string CubeWheel::GetLogHeader() const {
+  std::string str_tmp = "";
+  const std::string st_id = std::to_string(static_cast<long long>(port_id_));
+
+  str_tmp += WriteScalar("cubewheel_angular_velocity" + st_id, "rad/s");
+  str_tmp += WriteScalar("cubewheel_angular_velocity_rpm" + st_id, "rpm");
+  str_tmp += WriteScalar("cubewheel_angular_velocity_upperlimit" + st_id, "rpm");
+  str_tmp += WriteScalar("cubewheel_angular_acceleration" + st_id, "rad/s^2");
+
+  if (is_logged_jitter_) {
+    str_tmp += WriteVector("cubewheel_jitter_force" + st_id, "c", "N", 3);
+    str_tmp += WriteVector("cubewheel_jitter_torque" + st_id, "c", "Nm", 3);
+  }
+
+  return str_tmp;
+}
+
+int CubeWheel::GenerateTelemetry() {
+  unsigned char data[] = {
+      0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+  };
+
+  // state
+  unsigned char state = 0;
+  if (backup_mode_state_) {
+    state += (1 << 0);
+  }
+  if (motor_state_) {
+    state += (1 << 1);
+  }
+  if (hall_sensor_state_) {
+    state += (1 << 2);
+  }
+  if (encoder_state_) {
+    state += (1 << 3);
+  }
+  if (error_flag_) {
+    state += (1 << 4);
+  }
+
+  data[6] = control_mode_;
+  data[7] = state;
+  WriteRegister(kReadAddressWheelStatus_, data, 8);
+
+  // speed in rpm
+  double rpm = GetVelocityRpm();
+  int16_t val_speed_rpm = (int16_t)(rpm * 2);
+  data[1] = (unsigned char)(val_speed_rpm >> 8);
+  data[0] = (unsigned char)(val_speed_rpm);
+  WriteRegister(kReadAddressWheelData_, data, 6);
+
+  return 0;
+}

src/component/aocs/cube_wheel.h

@@ -0,0 +1,43 @@
+#ifndef S2E_AOBC_COMPONENT_AOCS_CUBE_WHEEL_HPP_
+#define S2E_AOBC_COMPONENT_AOCS_CUBE_WHEEL_HPP_
+
+#include "../../../s2e_core_oss/src/Component/AOCS/RWModel.h"
+#include "../../../s2e_core_oss/src/Component/Abstract/ObcI2cCommunicationBase.h"
+
+class CubeWheel : public RWModel, public ObcI2cCommunicationBase {
+ public:
+  CubeWheel(RWModel rw_model, const int port_id, const unsigned char i2c_addr, OBC* obc);
+
+  // Override: RWModel functions
+  void MainRoutine(int count) override;
+  std::string GetLogHeader() const override;
+
+ private:
+  // RWレジスタ
+  unsigned char control_mode_ = 0;
+  unsigned char backup_mode_state_ = 0;
+  unsigned char motor_state_ = 0;
+  unsigned char hall_sensor_state_ = 0;
+  unsigned char encoder_state_ = 0;
+  unsigned char error_flag_ = 0;
+
+  double lsb2nT_ = 13.0;
+  int port_id_ = 0;
+
+  // Write Command
+  static const uint8_t kWriteCmdControlMode_ = 10;
+  static const uint8_t kWriteCmdBackupMode_ = 12;
+
+  // Register address
+  static const uint8_t kReadAddressWheelStatus_ = 140;  // 本来は130
+  static const uint8_t kReadAddressWheelData_ = 150;    // 本来は137
+
+  static const uint8_t kReadAddressSpeed_ = 0x15;
+  static const uint8_t kReadAddressLimitSpeed1_ = 0x33;
+  static const uint8_t kReadAddressLimitSpeed2_ = 0x34;
+
+  // TLM
+  int32_t ConvertMag2Tlm(double mag);
+  int GenerateTelemetry();
+};
+#endif  // S2E_AOBC_COMPONENT_AOCS_CUBE_WHEEL_HPP_