Skip to content

Commit

Permalink
Merge pull request #509 from ut-issl/feature/add-first-order-lag
Browse files Browse the repository at this point in the history 
Refactor RW
  • Loading branch information
@200km
200km authored Oct 11, 2023
2 parents 3b8cf00 + 9ffb5d9 commit 1b7b859
Show file tree
Hide file tree
Showing 10 changed files with 343 additions and 286 deletions.
30 changes: 17 additions & 13 deletions data/sample/initialize_files/components/reaction_wheel.ini
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,9 +2,6 @@
// prescaler defines update period of thie component
// update period = prescaler * step sec
prescaler = 1
// prescaler for calculation of RW jitter
// To calculate the jitter, set the update cycle to about 0.1 ms.
fast_prescaler = 1

// Moment of inertia of the rotor [kgm^2]
moment_of_inertia_kgm2 = 1.0e-4
Expand Down Expand Up @@ -34,24 +31,31 @@ direction_b(2) = 0.0
position_b_m(0) = 1.0
position_b_m(1) = 0.0
position_b_m(2) = 0.0
// The control deray[s]
dead_time_s = 1.0
// Coefficient of first order lag for driving case
first_order_lag_coefficient(0) = 2.0
first_order_lag_coefficient(1) = 0.0
first_order_lag_coefficient(2) = 0.0
// Coefficient of first order lag for coasting case
coasting_lag_coefficient(0) = 2.0
coasting_lag_coefficient(1) = 0.0
coasting_lag_coefficient(2) = 0.0

// The control delay[s]
dead_time_s = 1.0 // Dead time [s]
time_constant_s = 2.0 // Time constant of first order lag [s]

// Coasting friction
// f_rad_s2 = v_rad_s * coefficients(0) + (v_rad_s)^2 * coefficients(1) + ...
friction_order = 2
friction_coefficients(0) = 0.1
friction_coefficients(1) = 0.1
friction_coefficients(2) = 0.0
stop_limit_angular_velocity_rad_s = 0.1

// For drive initialize
// They should be zero for normal case
initial_motor_drive_flag = 0
initial_angular_velocity_rad_s = 0.0

[REACTION_WHEEL_JITTER_1]
//Parameters for calculate RW jitter
jitter_calculation = DISABLE
jitter_logging = DISABLE
// prescaler for calculation of RW jitter
// To calculate the jitter, set the update cycle to about 0.1 ms.
fast_prescaler = 1
radial_force_harmonics_coefficient_file = INI_FILE_DIR_FROM_EXE/components/rw_disturbance_csv_files/radial_force_harmonics_coefficients.csv
radial_torque_harmonics_coefficient_file = INI_FILE_DIR_FROM_EXE/components/rw_disturbance_csv_files/radial_torque_harmonics_coefficients.csv
harmonics_degree = 12
Expand Down
7 changes: 6 additions & 1 deletion scripts/Plot/plot_reaction_wheel.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -51,11 +51,12 @@
angular_velocity_rad_s = read_scalar_from_csv(read_file_name, 'rw1_angular_velocity[rad/s]')
angular_velocity_rpm = read_scalar_from_csv(read_file_name, 'rw1_angular_velocity[rpm]')
angular_acceleration_rad_s2 = read_scalar_from_csv(read_file_name, 'rw1_angular_acceleration[rad/s2]')
target_angular_acceleration_rad_s2 = read_scalar_from_csv(read_file_name, 'rw1_target_angular_acceleration[rad/s2]')

#
# Plot
#
fig, axis = plt.subplots(3, 1, squeeze = False, tight_layout = True, sharex = True)
fig, axis = plt.subplots(4, 1, squeeze = False, tight_layout = True, sharex = True)
axis[0, 0].plot(time[0], angular_velocity_rad_s[0], marker=".", c="red", label="rad/s")
axis[0, 0].set(ylabel = "Angular velocity rad/s")
axis[0, 0].legend(loc = 'upper right')
Expand All @@ -68,6 +69,10 @@
axis[2, 0].set(ylabel = "Angular acceleration rad/s2")
axis[2, 0].legend(loc = 'upper right')

axis[3, 0].plot(time[0], target_angular_acceleration_rad_s2[0], marker=".", c="blue", label="rad/s2")
axis[3, 0].set(ylabel = "Target angular acceleration rad/s2")
axis[3, 0].legend(loc = 'upper right')

fig.suptitle("Reaction Wheel Output")
fig.supxlabel("Time [s]")

Expand Down
313 changes: 164 additions & 149 deletions src/components/real/aocs/reaction_wheel.cpp
View file

Large diffs are not rendered by default.

126 changes: 55 additions & 71 deletions src/components/real/aocs/reaction_wheel.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,7 @@
#ifndef S2E_COMPONENTS_REAL_AOCS_REACTION_WHEEL_HPP_
#define S2E_COMPONENTS_REAL_AOCS_REACTION_WHEEL_HPP_

#include <library/logger/loggable.hpp>
#include <library/control_utilities/first_order_lag.hpp>
#include <library/logger/logger.hpp>
#include <library/math/vector.hpp>
#include <limits>
Expand All @@ -29,78 +29,60 @@ class ReactionWheel : public Component, public ILoggable {
* @brief Constructor without power port
* @note TODO: argument is too long
* @param [in] prescaler: Frequency scale factor for update
* @param [in] fast_prescaler: Frequency scale factor for fast update
* @param [in] clock_generator: Clock generator
* @param [in] component_id: Component ID
* @param [in] step_width_s: Step width of integration by reaction wheel ordinary differential equation [sec]
* @param [in] main_routine_time_step_s: Period of execution of main routine of RW [sec]
* @param [in] jitter_update_interval_s: Update period of RW jitter [sec]
* @param [in] rotor_inertia_kgm2: Moment of rotor_inertia_kgm2 of the RW [kgm2]
* @param [in] max_torque_Nm: Maximum output torque [Nm]
* @param [in] max_velocity_rpm: Maximum output angular velocity [RPM]
* @param [in] quaternion_b2c: Quaternion from body frame to component frame
* @param [in] position_b_m: Position of RW on the body fixed frame [m]
* @param [in] dead_time_s: Dead time of torque output [sec]
* @param [in] driving_lag_coefficients: Driving lag coefficients
* @param [in] coasting_lag_coefficients: Coasting lag coefficients
* @param [in] time_constant_s: First order lag time constant [sec]
* @param [in] friction_coefficients: Friction coefficients
* @param [in] stop_limit_angular_velocity_rad_s: Angular velocity stop limit by friction [rad/s]
* @param [in] is_calc_jitter_enabled: Enable flag to calculate RW jitter
* @param [in] is_log_jitter_enabled: Enable flag to log output RW jitter
* @param [in] radial_force_harmonics_coefficients: Coefficients for radial force harmonics
* @param [in] radial_torque_harmonics_coefficients: Coefficients for radial torque harmonics
* @param [in] structural_resonance_frequency_Hz: Frequency of structural resonance [Hz]
* @param [in] damping_factor: Damping factor of structural resonance
* @param [in] bandwidth: Bandwidth of structural resonance
* @param [in] considers_structural_resonance: Flag to consider structural resonance
* @param [in] fast_prescaler: Frequency scale factor for fast update
* @param [in] rw_jitter: RW jitter
* @param [in] drive_flag: RW drive flag
* @param [in] init_velocity_rad_s: Initial value of angular velocity of RW
*/
ReactionWheel(const int prescaler, const int fast_prescaler, ClockGenerator* clock_generator, const int component_id, const double step_width_s,
const double main_routine_time_step_s, const double jitter_update_interval_s, const double rotor_inertia_kgm2,
const double max_torque_Nm, const double max_velocity_rpm, const libra::Quaternion quaternion_b2c,
const libra::Vector<3> position_b_m, const double dead_time_s, const libra::Vector<3> driving_lag_coefficients,
const libra::Vector<3> coasting_lag_coefficients, const bool is_calc_jitter_enabled, const bool is_log_jitter_enabled,
const std::vector<std::vector<double>> radial_force_harmonics_coefficients,
const std::vector<std::vector<double>> radial_torque_harmonics_coefficients, const double structural_resonance_frequency_Hz,
const double damping_factor, const double bandwidth, const bool considers_structural_resonance, const bool drive_flag = false,
ReactionWheel(const int prescaler, ClockGenerator* clock_generator, const int component_id, const double step_width_s,
const double rotor_inertia_kgm2, const double max_torque_Nm, const double max_velocity_rpm, const libra::Quaternion quaternion_b2c,
const libra::Vector<3> position_b_m, const double dead_time_s, const double time_constant_s,
const std::vector<double> friction_coefficients, const double stop_limit_angular_velocity_rad_s, const bool is_calc_jitter_enabled,
const bool is_log_jitter_enabled, const int fast_prescaler, ReactionWheelJitter& rw_jitter, const bool drive_flag = false,
const double init_velocity_rad_s = 0.0);
/**
* @fn ReactionWheel
* @brief Constructor with power port
* @param [in] prescaler: Frequency scale factor for update
* @param [in] fast_prescaler: Frequency scale factor for fast update
* @param [in] clock_generator: Clock generator
* @param [in] power_port: Power port
* @param [in] component_id: Component ID
* @param [in] step_width_s: Step width of integration by reaction wheel ordinary differential equation [sec]
* @param [in] main_routine_time_step_s: Period of execution of main routine of RW [sec]
* @param [in] jitter_update_interval_s: Update period of RW jitter [sec]
* @param [in] rotor_inertia_kgm2: Moment of rotor_inertia_kgm2 of the RW [kgm2]
* @param [in] max_torque_Nm: Maximum output torque [Nm]
* @param [in] max_velocity_rpm: Maximum output angular velocity [RPM]
* @param [in] quaternion_b2c: Quaternion from body frame to component frame
* @param [in] position_b_m: Position of RW on the body fixed frame [m]
* @param [in] dead_time_s: Dead time of torque output [sec]
* @param [in] driving_lag_coefficients: Driving lag coefficients
* @param [in] coasting_lag_coefficients: Coasting lag coefficients
* @param [in] time_constant_s: First order lag time constant [sec]
* @param [in] friction_coefficients: Friction coefficients
* @param [in] stop_limit_angular_velocity_rad_s: Angular velocity stop limit by friction [rad/s]
* @param [in] is_calc_jitter_enabled: Enable flag to calculate RW jitter
* @param [in] is_log_jitter_enabled: Enable flag to log output RW jitter
* @param [in] radial_force_harmonics_coefficients: Coefficients for radial force harmonics
* @param [in] radial_torque_harmonics_coefficients: Coefficients for radial torque harmonics
* @param [in] structural_resonance_frequency_Hz: Frequency of structural resonance [Hz]
* @param [in] damping_factor: Damping factor of structural resonance
* @param [in] bandwidth: Bandwidth of structural resonance
* @param [in] considers_structural_resonance: Flag to consider structural resonance
* @param [in] fast_prescaler: Frequency scale factor for fast update
* @param [in] rw_jitter: RW jitter
* @param [in] drive_flag: RW drive flag
* @param [in] init_velocity_rad_s: Initial value of angular velocity of RW [rad/s]
*/
ReactionWheel(const int prescaler, const int fast_prescaler, ClockGenerator* clock_generator, PowerPort* power_port, const int component_id,
const double step_width_s, const double main_routine_time_step_s, const double jitter_update_interval_s,
ReactionWheel(const int prescaler, ClockGenerator* clock_generator, PowerPort* power_port, const int component_id, const double step_width_s,
const double rotor_inertia_kgm2, const double max_torque_Nm, const double max_velocity_rpm, const libra::Quaternion quaternion_b2c,
const libra::Vector<3> position_b_m, const double dead_time_s, const libra::Vector<3> driving_lag_coefficients,
const libra::Vector<3> coasting_lag_coefficients, const bool is_calc_jitter_enabled, const bool is_log_jitter_enabled,
const std::vector<std::vector<double>> radial_force_harmonics_coefficients,
const std::vector<std::vector<double>> radial_torque_harmonics_coefficients, const double structural_resonance_frequency_Hz,
const double damping_factor, const double bandwidth, const bool considers_structural_resonance, const bool drive_flag = false,
const libra::Vector<3> position_b_m, const double dead_time_s, const double time_constant_s,
const std::vector<double> friction_coefficients, const double stop_limit_angular_velocity_rad_s, const bool is_calc_jitter_enabled,
const bool is_log_jitter_enabled, const int fast_prescaler, ReactionWheelJitter& rw_jitter, const bool drive_flag = false,
const double init_velocity_rad_s = 0.0);

// Override functions for Component
Expand Down Expand Up @@ -142,7 +124,7 @@ class ReactionWheel : public Component, public ILoggable {
* @fn GetJitterForce_b_N
* @brief Return output force by jitter in the body fixed frame [N]
*/
const libra::Vector<3> GetJitterForce_b_N() const { return rw_jitter_.GetJitterForce_b_N(); }
inline const libra::Vector<3> GetJitterForce_b_N() const;
/**
* @fn GetDriveFlag
* @brief Return drive flag
Expand Down Expand Up @@ -188,43 +170,46 @@ class ReactionWheel : public Component, public ILoggable {

protected:
// Fixed Parameters
const int component_id_; //!< Actuator ID
const double rotor_inertia_kgm2_; //!< Inertia of RW rotor [kgm2]
const double max_torque_Nm_; //!< Maximum output torque [Nm]
const double max_velocity_rpm_; //!< Maximum angular velocity of rotor [rpm]
libra::Quaternion quaternion_b2c_; //!< Quaternion from body frame to component frame
const libra::Vector<3> position_b_m_; //!< Position of RW in the body fixed frame [m]
libra::Vector<3> rotation_axis_c_; //!< Wheel rotation axis on the component frame. Constant as (0 0 1). (Output torque is minus direction)
libra::Vector<3> rotation_axis_b_; //!< Wheel rotation vector in the body fixed frame.
const int component_id_; //!< Actuator ID
const double rotor_inertia_kgm2_; //!< Inertia of RW rotor [kgm2]
const double max_torque_Nm_; //!< Maximum output torque [Nm]
const double max_velocity_rpm_; //!< Maximum angular velocity of rotor [rpm]
const libra::Quaternion quaternion_b2c_; //!< Quaternion from body frame to component frame
const libra::Vector<3> position_b_m_; //!< Position of RW in the body fixed frame [m]
libra::Vector<3> rotation_axis_c_; //!< Wheel rotation axis on the component frame. Constant as (0 0 1). (Output torque is minus direction)
libra::Vector<3> rotation_axis_b_; //!< Wheel rotation vector in the body fixed frame.

// Parameters for control delay
const double step_width_s_; //!< step width for ReactionWheelOde [sec]
const double dead_time_s_; //!< dead time [sec]
std::vector<double> acceleration_delay_buffer_; //!< Delay buffer for acceleration
FirstOrderLag delayed_acceleration_rad_s2_; //!< Delayed acceleration [rad/s2]

// Fixed Parameters for control delay
const double step_width_s_; //!< step width for ReactionWheelOde [sec]
const double dead_time_s_; //!< dead time [sec]
const libra::Vector<3> driving_lag_coefficients_; //!< delay coefficient for normal drive
const libra::Vector<3> coasting_lag_coefficients_; //!< delay coefficient for coasting drive(Power off)
// Coasting friction
// f_rad_s2 = v_rad_s * coefficients(0) + (v_rad_s)^2 * coefficients(1) + ...
std::vector<double> coasting_friction_coefficients_; //!< Friction coefficients for coasting
double stop_limit_angular_velocity_rad_s_ = 0.1; //!< Angular velocity stop limit by friction [rad/s]

// Controlled Parameters
bool drive_flag_; //!< Drive flag(True: Drive, False: Stop)
double velocity_limit_rpm_; //!< Velocity limit defined by users [RPM]
bool drive_flag_; //!< Drive flag (True: Drive, False: Stop)
double target_acceleration_rad_s2_; //!< Target acceleration [rad/s2]

// Internal variables for control delay
std::vector<double> acceleration_delay_buffer_; //!< Delay buffer for acceleration
double main_routine_time_step_s_; //!< Period of execution of main routine [sec]

// Output at RW frame
double angular_acceleration_rad_s2_ = 0.0; //!< Output angular acceleration [rad/s2]
double angular_velocity_rpm_ = 0.0; //!< Current angular velocity [rpm]
double angular_velocity_rad_s_ = 0.0; //!< Current angular velocity [rad/s]

double generated_angular_acceleration_rad_s2_ = 0.0; //!< Generated acceleration [rad/s2]
double angular_velocity_rpm_ = 0.0; //!< Current angular velocity [rpm]
double angular_velocity_rad_s_ = 0.0; //!< Current angular velocity [rad/s]
// Output at body frame
libra::Vector<3> output_torque_b_Nm_{0.0}; //!< Output torque in the body fixed frame [Nm]
libra::Vector<3> angular_momentum_b_Nms_{0.0}; //!< Angular momentum of RW [Nms]

// ODE
double velocity_limit_rpm_; //!< Velocity limit defined by users [RPM]
ReactionWheelOde ode_angular_velocity_; //!< Reaction Wheel OrdinaryDifferentialEquation
ReactionWheelJitter rw_jitter_; //!< RW jitter
bool is_calculated_jitter_ = false; //!< Flag for calculation of jitter
bool is_logged_jitter_ = false; //!< Flag for log output of jitter

// RW jitter
ReactionWheelJitter& rw_jitter_; //!< RW jitter
bool is_calculated_jitter_ = false; //!< Flag for calculation of jitter
bool is_logged_jitter_ = false; //!< Flag for log output of jitter

// Local functions
/**
Expand All @@ -245,10 +230,9 @@ class ReactionWheel : public Component, public ILoggable {
* @param [in] clock_generator: Clock generator
* @param [in] actuator_id: Actuator ID
* @param [in] file_name: Path to the initialize file
* @param [in] prop_step: Propagation step for RW dynamics [sec]
* @param [in] compo_update_step: Component step time [sec]
* @param [in] compo_update_step_s: Component step time [sec]
*/
ReactionWheel InitReactionWheel(ClockGenerator* clock_generator, int actuator_id, std::string file_name, double prop_step, double compo_update_step);
ReactionWheel InitReactionWheel(ClockGenerator* clock_generator, int actuator_id, std::string file_name, double compo_update_step_s);
/**
* @fn InitReactionWheel
* @brief Initialize functions for reaction wheel with power port
Expand All @@ -257,9 +241,9 @@ ReactionWheel InitReactionWheel(ClockGenerator* clock_generator, int actuator_id
* @param [in] actuator_id: Actuator ID
* @param [in] file_name: Path to the initialize file
* @param [in] prop_step: Propagation step for RW dynamics [sec]
* @param [in] compo_update_step: Component step time [sec]
* @param [in] compo_update_step_s: Component step time [sec]
*/
ReactionWheel InitReactionWheel(ClockGenerator* clock_generator, PowerPort* power_port, int actuator_id, std::string file_name, double prop_step,
double compo_update_step);
ReactionWheel InitReactionWheel(ClockGenerator* clock_generator, PowerPort* power_port, int actuator_id, std::string file_name,
double compo_update_step_s);

#endif // S2E_COMPONENTS_REAL_AOCS_REACTION_WHEEL_HPP_
7 changes: 6 additions & 1 deletion src/components/real/aocs/reaction_wheel_jitter.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,11 @@
*/
class ReactionWheelJitter {
public:
/**
* @fn ReactionWheelJitter
* @brief Default Constructor
*/
ReactionWheelJitter() {}
/**
* @fn ReactionWheelJitter
* @brief Constructor
Expand Down Expand Up @@ -75,7 +80,7 @@ class ReactionWheelJitter {
std::vector<std::vector<double>> radial_force_harmonics_coefficients_; //!< Coefficients for radial force harmonics
std::vector<std::vector<double>> radial_torque_harmonics_coefficients_; //!< Coefficients for radial torque harmonics

const double update_interval_s_; //!< Jitter update interval [sec]
double update_interval_s_; //!< Jitter update interval [sec]
libra::Quaternion quaternion_b2c_; //!< Quaternion from body frame to component frame

double structural_resonance_frequency_Hz_; //!< Frequency of structural resonance [Hz]
Expand Down
Loading

0 comments on commit 1b7b859

Please sign in to comment.