drive_sim_basic.h

/*
 * Developer : Mark Verrijt (mark.verrijt@vectioneer.com)
 * All rights reserved. Copyright (c) 2019 VECTIONEER.
 */
 
#ifndef MOTORCORTEX_LIB_DRIVE_SIM_BASE_H
#define MOTORCORTEX_LIB_DRIVE_SIM_BASE_H
 
#include "ct_module.h"
 
namespace mcx {
 
namespace drive {
 
namespace sim {
 
template<typename T>
class DriveBasic : public mcx::container::Module {
public:
  explicit DriveBasic(unsigned int num_channels = 1) : num_channels_(num_channels) {
    target_position_ = new T[num_channels_]{};
    target_torque_ = new T[num_channels_]{};
    target_torque_offset_ = new T[num_channels_]{};
 
    position_value_ = new T[num_channels_]{};
    torque_value_ = new T[num_channels_]{};
    torque_sensor_value_ = new T[num_channels_]{};
 
    position_value_init_ = new T[num_channels_]{};
 
    drive_on_ = new bool[num_channels_]{};
  }
 
  ~DriveBasic() override {
    delete[] target_position_;
    delete[] target_torque_;
    delete[] target_torque_offset_;
 
    delete[] position_value_;
    delete[] torque_value_;
    delete[] torque_sensor_value_;
 
    delete[] position_value_init_;
 
    delete[] drive_on_;
  };
 
private:
 
  static constexpr unsigned int init_cycles_ = 100;
 
  void create_(const char* name, mcx::parameter_server::Parameter* parameter_server, uint64_t dt_micro_s) override {};
 
  bool initPhase1_() override {
    addParameter("targetPosition", mcx::parameter_server::ParameterType::INPUT, target_position_, num_channels_);
    addParameter("targetTorque", mcx::parameter_server::ParameterType::INPUT, target_torque_, num_channels_);
    addParameter("targetTorqueOffset", mcx::parameter_server::ParameterType::INPUT, target_torque_offset_,
                 num_channels_);
    addParameter("driveOn", mcx::parameter_server::ParameterType::INPUT, drive_on_, num_channels_);
    addParameter("positionValueInit", mcx::parameter_server::ParameterType::INPUT, position_value_init_, num_channels_);
    addParameter("positionValue", mcx::parameter_server::ParameterType::OUTPUT, position_value_, num_channels_);
    addParameter("torqueValue", mcx::parameter_server::ParameterType::OUTPUT, torque_value_, num_channels_);
    addParameter("torqueSensorValue", mcx::parameter_server::ParameterType::OUTPUT, torque_sensor_value_,
                 num_channels_);
    return true;
  };
 
  bool initPhase2_() override {
    return true;
  };
 
  bool startOp_() override {
    return true;
  };
 
  bool stopOp_() override {
    return true;
  };
 
  bool iterateOp_(const mcx::container::TaskTime& system_time, mcx::container::UserTime* user_time) override {
    if (system_time.nr_of_cycles <= init_cycles_) {
      memcpy(position_value_, position_value_init_, num_channels_ * sizeof(T));
    }
    for (unsigned int cnt = 0; cnt < num_channels_; ++cnt) {
      if (drive_on_[cnt]) {
        position_value_[cnt] = target_position_[cnt];
 
        // Not entirely correct but should work:
        torque_value_[cnt] = target_torque_[cnt] + target_torque_offset_[cnt];
        torque_sensor_value_[cnt] = torque_value_[cnt];
      } else {
        // Keep position fixed where it is
 
        // Torque = 0
        torque_value_[cnt] = 0;
        torque_sensor_value_[cnt] = 0;
      }
    }
    return true;
  };
 
private:
  const unsigned int num_channels_;
  bool* drive_on_;
 
  T* position_value_init_;
 
  T* target_position_;
  T* target_torque_;
  T* target_torque_offset_;
 
  T* position_value_;
  T* torque_value_;
  T* torque_sensor_value_;
 
};
 
}  // namespace sim
 
}  // namespace drive
 
}  // namespace mcx
 
#endif /* MOTORCORTEX_LIB_DRIVE_SIM_BASE_H */