sine_position_control.hpp

Go to the documentation of this file.

#include "blmc_drivers/devices/motor.hpp"
#include "blmc_drivers/devices/analog_sensor.hpp"

namespace blmc_drivers{

typedef std::shared_ptr<blmc_drivers::SafeMotor> SafeMotor_ptr;

class SinePositionControl
{
public:
  SinePositionControl(std::vector<SafeMotor_ptr> motor_list):
    motor_list_(motor_list)
  {
    encoders_.clear();
    velocities_.clear();
    currents_.clear();
    control_buffer_.clear();

    for(size_t i=0 ; i<motor_list.size() ; ++i)
    {
      encoders_.push_back(std::deque<double>());
      currents_.push_back(std::deque<double>());
      velocities_.push_back(std::deque<double>());
      control_buffer_.push_back(std::deque<double>());
      encoders_.back().clear();
      velocities_.back().clear();
      currents_.back().clear();
      control_buffer_.back().clear();
    }
    stop_loop_=false;
    kp_ = 0.0;
    kd_ = 0.0;
  }

  ~SinePositionControl()
  {
    stop_loop_=true;
    rt_thread_.join();
  }

  void start_loop()
  {
    rt_thread_.create_realtime_thread(&SinePositionControl::loop, this);
  }

  void stop_loop();

  void set_gains(double kp, double kd)
  {
    kp_ = kp;
    kd_ = kd;
  }

private:

  std::vector<SafeMotor_ptr> motor_list_;
  real_time_tools::RealTimeThread rt_thread_;

  static THREAD_FUNCTION_RETURN_TYPE loop(void* instance_pointer)
  {
    ((SinePositionControl*)(instance_pointer))->loop();
    return THREAD_FUNCTION_RETURN_VALUE;
  }

  void loop();

  bool stop_loop_;

  unsigned memory_buffer_size_;

  std::vector<std::deque<double> > encoders_;
  
  std::vector<std::deque<double> > velocities_;

  std::vector<std::deque<double> > currents_;

  std::vector<std::deque<double> > control_buffer_;

  double kp_;

  double kd_;

}; // end class SinePositionControl definition

}// namespace blmc_drivers