profile.h

Go to the documentation of this file.

#pragma once
 
#include <chrono>
#include <cstdint>
#include <functional>
 
#include "util.h"
 
typedef struct {
  double max_acceleration;  // max acceleration
  double max_velocity;
  int32_t max_torque;               // permill
  int32_t max_torque_acceleration;  // permill/s
 
  /*User Inputs*/
 
  double acc;  // acceleration
  double dec;  // deceleration
  double vi;   // velocity
  double qi;   // initial position
  double qf;   // final position
 
  /*Profile time*/
 
  double T;       // total no. of Samples
  double s_time;  // sampling time
 
  int32_t direction;
 
  /*LFPB motion profile constants*/
 
  double ai;
  double bi;
  double ci;
  double di;
  double ei;
  double fi;
  double gi;
 
  /*internal velocity variables*/
 
  double qid;  // initial velocity
  double qfd;  // final velocity
 
  double distance_cruise;
  double total_distance;
  double distance_acc;  // distance covered during acceleration
  double distance_dec;  // distance covered during deceleration
 
  double tb_acc;    // blend time for acceleration profile
  double tb_dec;    // blend time for deceleration profile
  double tf;        // total time for profile
  double t_cruise;  // time for cruise velocity profile
  double ts;        // variable to hold current sample time
 
  double q;  // position profile
 
  double max_position;
  double min_position;
 
  int32_t steps;  // number of steps required to execute this profile
} MotionProfile;
 
class Profile {
 public:
  Profile(Cia402Drive& cia402_drive, int64_t target, uint32_t velocity,
          uint32_t acceleration, uint32_t deceleration, uint32_t torque_slope,
          float sine_frequency, uint32_t sustain_time_ms, bool repeat,
          bool master_generated = false, bool absolute_target = false);
 
  ~Profile();

  MotionMasterError start_ramp_position_profile(
      std::function<void(MotionMasterWarning)> warning_callback = nullptr);

  MotionMasterError start_trapezoidal_position_profile(
      std::function<void(MotionMasterWarning)> warning_callback = nullptr);

  MotionMasterError start_bidirectional_position_profile(
      std::function<void(MotionMasterWarning)> warning_callback = nullptr);

  MotionMasterError start_sine_wave_position_profile();

  MotionMasterError start_ramp_velocity_profile(
      std::function<void(MotionMasterWarning)> warning_callback = nullptr,
      bool non_stop = false);

  MotionMasterError start_trapezoidal_velocity_profile(
      std::function<void(MotionMasterWarning)> warning_callback = nullptr);

  MotionMasterError start_bidirectional_velocity_profile(
      std::function<void(MotionMasterWarning)> warning_callback = nullptr);

  MotionMasterError start_sine_wave_velocity_profile();

  MotionMasterError start_ramp_torque_profile(
      std::function<void(MotionMasterWarning)> warning_callback = nullptr);

  MotionMasterError start_trapezoidal_torque_profile(
      std::function<void(MotionMasterWarning)> warning_callback = nullptr);

  MotionMasterError start_bidirectional_torque_profile(
      std::function<void(MotionMasterWarning)> warning_callback = nullptr);

  MotionMasterError start_sine_wave_torque_profile();

  void stop();

  bool was_aborted() const;
 
 private:
  Cia402Drive& cia402_drive_;
  MotionProfile motion_profile_ = {};
  int32_t target_;
  uint32_t velocity_;
  uint32_t acceleration_;
  uint32_t deceleration_;
  uint32_t torque_slope_;
  uint32_t sustain_time_ms_;
 
  uint32_t encoder_resolution_ = 0;
  double feed_constant_ = 0;
  int32_t position_delta_ = 0;
 
  bool repeat_;
  bool absolute_target_;
  bool stop_ = false;
  bool aborted_ = false;
 
  // Master generated profile values
  bool master_generated_;
  int32_t min_position_range_limit_ = 0;
  int32_t max_position_range_limit_ = 0;
  float sine_frequency_;
 
  uint32_t object_id_;

  MotionMasterError init_master_position_profile();

  MotionMasterError init_master_velocity_profile();

  MotionMasterError init_master_torque_profile();
 
  bool is_slave_position_profile_supported();
  bool is_slave_velocity_profile_supported();
  bool is_slave_torque_profile_supported();
  MotionMasterError setup_slave_position_profile(
      std::function<void(MotionMasterWarning)>& warning_callback,
      bool bidirectional = false);
  MotionMasterError setup_slave_velocity_profile(
      std::function<void(MotionMasterWarning)>& warning_callback);
  MotionMasterError setup_slave_torque_profile(
      std::function<void(MotionMasterWarning)>& warning_callback);

  void deinit_master_profile();

  void execute_master_position_profile_stage(int32_t start, int32_t target);

  void execute_master_position_profile_sin(int32_t start, int32_t target,
                                           bool repeat);

  void execute_master_velocity_profile_stage(int32_t start, int32_t target);

  void execute_master_velocity_profile_sin(int32_t start, int32_t target,
                                           bool repeat);

  void execute_master_torque_profile_stage(int32_t start, int32_t target);

  void execute_master_torque_profile_sin(int32_t start, int32_t target,
                                         bool repeat);

  int32_t calculate_position_profile_steps(int32_t start, int32_t target);

  int32_t calculate_velocity_profile_steps(int32_t start, int32_t target);

  int32_t calculate_torque_profile_steps(int32_t start, int32_t target);

  void init_master_profile_limits(int32_t max_torque_acceleration,
                                  int32_t max_acceleration,
                                  int32_t max_position, int32_t min_position);

  int32_t init_linear_profile();

  int32_t position_profile_generate(int32_t step);

  int32_t linear_profile_generate_in_steps(int32_t step);

  static double rpm_to_ticks(uint32_t rpm, uint32_t encoder_resolution);

  static double ticks_to_rpm(double ticks, uint32_t encoder_resolution);

  void validate_position_profile_acceleration_deceleration();

  void interruptible_sustain(uint32_t sustain_time_ms);
};