narrow_angle_calibration.h

Go to the documentation of this file.

#pragma once
 
#include <MU_3SL_defs.h>
#include <MU_3SL_interface.h>
 
#include <chrono>
#include <future>
#include <optional>
 
#include "machine_procedure.h"
#include "motion-master.pb.h"
#include "motion_master_error.h"
#include "util.h"
 
// Automatic gains registers
#define AC_AF_GAIN_M_ADDRESS 0x2B
#define AC_AF_GAIN_N_ADDRESS 0x2F
 
// Registers necessary to set raw mode
#define MODE_ST_ADDRESS 0x12
#define OUT_ZERO_OUT_MSB_ADDRESS 0x11
// Sinus and Cosinus offset registers
#define VOSS_M_ADDRESS 0x02
#define VOSS_N_ADDRESS 0x08
#define VOSC_M_ADDRESS 0x03
#define VOSC_N_ADDRESS 0x09
// Multiturn interface
#define MT_INTERFACE_ADDRESS 0x10
// Phase adjustment
#define PH_M_ADDRESS 0x04
#define PH_N_ADDRESS 0x0A
// Cosine gain
#define GX_M_ADDRESS 0x01
#define GX_N_ADDRESS 0x07
 
// Command register
#define CMD_MU_ADDRESS 0x75
#define STATUS0_ADDRESS 0x76
#define STATUS1_ADDRESS 0x77
#define WRITE_ALL 0x01
#define CRC_CALC 0x0E
 
#define SPO_0_BASE_ADDRESS 0x52
#define SPO_1_2_ADDRESS 0x53
#define SPO_3_4_ADDRESS 0x54
#define SPO_5_6_ADDRESS 0x55
#define SPO_7_8_ADDRESS 0x56
#define SPO_9_10_ADDRESS 0x57
#define SPO_11_12_ADDRESS 0x58
#define SPO_13_14_ADDRESS 0x59
 
#define CRC16_ADDRESS_0 0x22
#define CRC16_ADDRESS_1 0x21
#define MPC_ADDRESS 0x0F
#define HARD_REV_ADDRESS 0x74
 
#define HRD_DATA_FILE_SIZE 8000u
 
class Cia402Drive;
class SiUnitVelocity;
 
typedef struct {
  long int sine_offset;
  long int cosine_offset;
  long int phase;
  uint32_t PHR;
  long int gain;
} CalibrationParameters;
 
class NarrowAngleCalibration : public MachineProcedure {
 public:
  NarrowAngleCalibration(uint32_t device_address, Cia402Drive& cia402_drive,
                         uint8_t encoder_ordinal,
                         bool activate_health_monitoring, bool measurement_only,
                         uint32_t external_encoder_type_,
                         int32_t velocity_base_value_rpm = 0);
 
  ~NarrowAngleCalibration() override;
 
  std::optional<MotionMasterError> execute(uint64_t pending_signals) override;

  std::pair<int64_t, double> get_magnet_distance(uint32_t ring_revision);
 
 private:
  enum class CalibrationStage {
    kInit = 0,
    kInitIteration,
    kDeinitIteration,
    kCalibration,
    kValidation,
    kDeinit,
    kWriteEeprom,
    kAbsReset,
    kFinal,
    kDone
  };
 
  uint8_t encoder_ordinal_;
  bool activate_health_monitoring_;
  bool measurement_only_;
  uint32_t external_encoder_type_;
  int32_t velocity_base_value_rpm_;
  CirculoEncoderType encoder_type_;
  int32_t hardware_id_;
  int32_t multiturn_resolution_;
  uint8_t default_msb_out_;
 
  bool biss_reset_required_ = false;
 
  // Used only for encoder port 2 and encoder functions COMMUTATION_AND_MOTION
  // and COMMUTATION_AND_VELOCITY
  double calibration_velocity_base_value_rpm_;
  double calibration_velocity_rpm_;
 
  // Limited range motion encoder calibration
  int32_t min_software_position_limit_;
  int32_t max_software_position_limit_;
  uint32_t position_encoder_singleturn_resolution_;
  int32_t starting_position_;
  double profile_acceleration_rpm_;
  double start_recording_demand_velocity_rpm_;
 
  double max_motor_speed_rpm_;
  double gear_ratio_;
  double feed_constant_;
 
  EncoderLocation encoder_location_ = EncoderLocation::kUnspecified;
 
  bool commutation_offset_measurement_possible_ = false;
 
  MU_Handle mu_handle_;
 
  MU_Calibration* mu_calibration_ = nullptr;
 
  MU_CalibrationAnalyzeResult* mu_analyze_result_ = nullptr;
 
  // Iteration data
  bool master_track_calibration_successful_ = false;
  bool nonius_track_calibration_successful_ = false;
  int32_t iteration_number_ = 0;
  int32_t max_iterations_ = 13;
  size_t number_of_samples_ = 6000;
  size_t number_of_hrd_data_files_;
 
  CalibrationStage calibration_stage_ = CalibrationStage::kInit;
 
  bool validation_iteration_ = false;
 
  std::optional<std::future<MotionMasterError>> future_;
 
  MotionMasterError error_;
 
  std::unique_ptr<SiUnitVelocity> si_unit_velocity_;
 
  static constexpr float coarse_gain_values_[4] = {4.4f, 7.8f, 12.4f, 20.7f};
 
  // DEBUG variables
  std::vector<std::pair<CalibrationParameters, CalibrationParameters>>
      calibration_parameter_store_;
 
  const std::vector<std::map<CirculoEncoderType, std::pair<int32_t, int32_t>>>
      magnet_distance_amplification_limits_ = {
          {// Default ring - revision 0 (the same as Hutchinson at the moment)
           {CirculoEncoderType::kCirculo7Inner, {14, 290}},
           {CirculoEncoderType::kCirculo7Outer, {12, 94}},
           {CirculoEncoderType::kCirculo9Inner, {14, 290}},
           {CirculoEncoderType::kCirculo9Outer, {12, 194}}},
          {// Bogen ring - revision 1
           {CirculoEncoderType::kCirculo7Inner, {24, 280}},
           {CirculoEncoderType::kCirculo7Outer, {9, 75}},
           {CirculoEncoderType::kCirculo9Inner, {24, 280}},
           {CirculoEncoderType::kCirculo9Outer, {13, 134}}},
          {// Hutchinson ring - revision 2
           {CirculoEncoderType::kCirculo7Inner, {14, 290}},
           {CirculoEncoderType::kCirculo7Outer, {12, 94}},
           {CirculoEncoderType::kCirculo9Inner, {14, 290}},
           {CirculoEncoderType::kCirculo9Outer, {12, 194}}}};
 
  const std::vector<std::map<CirculoEncoderType, std::vector<double>>>
      magnet_distance_polynomial_coefficients_ = {
          {// Default ring - revision 0 (the same as Hutchinson at the moment)
           {CirculoEncoderType::kCirculo7Inner,
            {-1.93733026150236881113e-01, 1.84893973187097715449e-02,
             -1.27277290997118431833e-04, 4.27826067036225706997e-07,
             -5.20368296543940458560e-10, 0, 0}},
           {CirculoEncoderType::kCirculo7Outer,
            {-1.87769498023411640641e-01, 1.27635840217653444284e-02,
             6.14758921668711374314e-04, -1.34096326750597602447e-05,
             7.56425020103532749652e-08, 0, 0}},
           {CirculoEncoderType::kCirculo9Inner,
            {-1.04397317851772072905e-01, 1.70625714417142575197e-02,
             -8.13961183382052607229e-05, -4.74936939216182293326e-07,
             7.04504263424913605881e-09, -2.76371817076000863454e-11,
             3.64854567222488168656e-14}},
           {CirculoEncoderType::kCirculo9Outer,
            {-2.84403156637705600840e-01, 3.12795823555360244517e-02,
             -3.25259935985799334551e-04, 1.81647913872471759832e-06,
             -4.91288513836093814287e-09, 5.10776292998975389867e-12, 0}}},
          {// Bogen ring - revision 1
           {CirculoEncoderType::kCirculo7Inner,
            {-1.39784903650814168463e-01, 1.45325823633666237344e-02,
             -7.50005188278302195896e-05, 1.61253405153954890157e-07,
             -4.89306413790261647012e-11, 0, 0}},
           {CirculoEncoderType::kCirculo7Outer,
            {-1.81692029808327909501e-01, 3.02858503823710337177e-02,
             1.23175956046672112430e-04, -8.59123594643554747716e-06,
             6.60504319801355069769e-08, 0, 0}},
           {CirculoEncoderType::kCirculo9Inner,
            {-3.00602095387669798754e-01, 2.34428435042899098451e-02,
             -2.62627767128114210458e-04, 1.69123403634972594129e-06,
             -5.27050975872971237842e-09, 5.33125270236813597257e-12,
             3.18588067695482955623e-15}},
           {CirculoEncoderType::kCirculo9Outer,
            {-1.66308923049894613611e-01, 1.82202872259577333680e-02,
             3.95115033121136774701e-04, -1.41407239990230904438e-05,
             1.61906309244842605822e-07, -8.18318393262045254823e-10,
             1.54906578095712552649e-12}}},
          {// Hutchinson ring - revision 2
           {CirculoEncoderType::kCirculo7Inner,
            {-1.93733026150236881113e-01, 1.84893973187097715449e-02,
             -1.27277290997118431833e-04, 4.27826067036225706997e-07,
             -5.20368296543940458560e-10, 0, 0}},
           {CirculoEncoderType::kCirculo7Outer,
            {-1.87769498023411640641e-01, 1.27635840217653444284e-02,
             6.14758921668711374314e-04, -1.34096326750597602447e-05,
             7.56425020103532749652e-08, 0, 0}},
           {CirculoEncoderType::kCirculo9Inner,
            {-1.04397317851772072905e-01, 1.70625714417142575197e-02,
             -8.13961183382052607229e-05, -4.74936939216182293326e-07,
             7.04504263424913605881e-09, -2.76371817076000863454e-11,
             3.64854567222488168656e-14}},
           {CirculoEncoderType::kCirculo9Outer,
            {-2.84403156637705600840e-01, 3.12795823555360244517e-02,
             -3.25259935985799334551e-04, 1.81647913872471759832e-06,
             -4.91288513836093814287e-09, 5.10776292998975389867e-12, 0}}}};
 
  MotionMasterError init();
 
  MotionMasterError init_next_iteration();
 
  MotionMasterError deinit_iteration();
 
  MotionMasterError calibration();
 
  MotionMasterError deinit();
 
  MotionMasterError set_biss_to_raw_mode();
 
  MotionMasterError reset_biss_to_default();
 
  MotionMasterError get_and_check_software_position_limits();
 
  MotionMasterError initialize_internal_values();
 
  void set_default_msb_out();
 
  MotionMasterError set_default_calibration();

  MU_Error analyze_and_compute_analog_adjustments(
      const uint16_t* master_data, const uint16_t* nonius_data,
      int number_of_samples, CalibrationParameters& master_params,
      CalibrationParameters& nonius_params);

  std::pair<MotionMasterError, MU_Error> write_master_track_analog_parameters();

  std::pair<MotionMasterError, MU_Error> write_nonius_track_analog_parameters();

  MotionMasterError initialize_calibration_engine(MU_Handle mu_handle);

  static bool is_master_analog_within_tolerance(
      CalibrationParameters calibration_parameters, uint8_t precision);

  static bool is_nonius_analog_within_tolerance(
      CalibrationParameters calibration_parameters, uint8_t precision);

  std::pair<MotionMasterError, MU_Error> write_nonius_sync_table();

  MotionMasterError commit_parameters_to_eeprom();

  MotionMasterError apply_hardware_reset();

  MotionMasterError finalize_calibration();

  void print_calibration_summary();
 
  void write_final_nonius_data_debug_file();

  MU_Error apply_optimized_nonius_table();

  void log_all_calibration_parameters();

  void log_internal_calibration_state();
 
  MotionMasterError config_hrd_stream_recording();
 
  MotionMasterError start_hrd_stream_recording();
 
  void limit_calibration_velocity(double max_calibration_velocity_rpm,
                                  bool notify = false);
 
  MotionMasterError ignore_biss_status_bits(bool ignore);
 
  MotionMasterError commutation_offset_measurement();
 
  MotionMasterError adjust_position(int32_t target_position);
 
  bool commutation_offset_measurement_possible();
 
  int32_t convert_angle_to_position_units(double angle) const;
};