system_identification.h

Go to the documentation of this file.

#pragma once
 
#include <vector>
 
#include "cia402_drive.h"
#include "motion-master.pb.h"
#include "si_unit_velocity.h"
#include "util.h"
 
class SystemIdentification {
 public:
  SystemIdentification(
      uint32_t device_address, Cia402Drive& cia402_drive,
      TuningParameters tuning_params,
      const std::function<void()>& started_callback = {},
      const std::function<void(motionmaster::MotionMasterMessage::Status::
                                   SystemIdentification::Warning::Code,
                               std::string)>& warning_callback = {},
      bool skip_chirp_signal = false);
 
  ~SystemIdentification();

  std::variant<MotionMasterError, PlantModel> start();
 
 private:
  uint32_t device_address_;
  Cia402Drive& cia402_drive_;
 
  TuningParameters tuning_params_;
  SystemParameters system_params_;
 
  const std::function<void()>& started_callback_;
  const std::function<void(motionmaster::MotionMasterMessage::Status::
                               SystemIdentification::Warning::Code,
                           std::string)>& warning_callback_;
 
  SiUnitVelocity si_unit_velocity_;
 
  bool skip_chirp_signal_ = false;
 
  double gear_ratio_ = 1.0;
  double feed_constant_ = 1.0;

  MotionMasterError init();

  MotionMasterError save_plant_model_to_flash(PlantModel m);

  MotionMasterError check_input_signal_amplitudes(
      const std::vector<int32_t>& torques,
      const std::vector<double>& velocities);

  MotionMasterError set_integral_limits();

  std::vector<double> convert_velocity_q15_to_si(
      const std::vector<int32_t>& q15_velocities);

  static std::vector<double> convert_torque_to_si(
      const std::vector<int32_t>& raw_torques, uint32_t nominal_torque);

  static bool check_model_stability(const PlantModel& m);
 
  static void write_sys_id_csv(const std::string& filename,
                               const std::vector<double>& torques_si,
                               const std::vector<double>& velocities_si);
};