motor.h

Go to the documentation of this file.

#ifndef MOTOR_H
#define MOTOR_H

#include <ros/ros.h>
#include <urdf/model.h>

#include <diagnostic_updater/diagnostic_updater.h>
#include <diagnostic_updater/publisher.h>
#include <hardware_interface/joint_state_interface.h>
#include <hardware_interface/joint_command_interface.h>
#include <joint_limits_interface/joint_limits_interface.h>

#include <roboteq_control/MotorStatus.h>
#include <roboteq_control/ControlStatus.h>

#include "roboteq/serial_controller.h"
#include "configurator/gpio_sensor.h"
#include "configurator/motor_param.h"
#include "configurator/motor_pid.h"

namespace roboteq
{

typedef struct _motor_status {
    uint8_t amps_limit : 1;
    uint8_t motor_stalled : 1;
    uint8_t loop_error_detect : 1;
    uint8_t safety_stop_active : 1;
    uint8_t forward_limit_triggered : 1;
    uint8_t reverse_limit_triggered : 1;
    uint8_t amps_triggered_active : 1;
    uint8_t : 1;
} motor_status_t;

class Motor : public diagnostic_updater::DiagnosticTask
{
public:
    explicit Motor(const ros::NodeHandle &nh, serial_controller *serial, string name, unsigned int number);
    void initializeMotor(bool load_from_board);
    void run(diagnostic_updater::DiagnosticStatusWrapper &stat);
    void setupLimits(urdf::Model model);
    void resetPosition(double position);
    void writeCommandsToHardware(ros::Duration period);
    void switchController(string type);
    void stopMotor();
    int getNumber() {
        return mNumber;
    }

    string getName() {
        return mMotorName;
    }
    void registerSensor(GPIOSensor* sensor)
    {
        _sensor = sensor;
    }
    void readVector(std::vector<std::string> fields);

    hardware_interface::JointStateHandle joint_state_handle;
    hardware_interface::JointHandle joint_handle;

    // Number of motor
    unsigned int mNumber;
protected:
  static double to_rpm(double x)
  {
    return x * 60 / (2 * M_PI);
  }

  static double from_rpm(double x)
  {
    return x * (2 * M_PI) / 60;
  }

  double to_encoder_ticks(double x);

  double from_encoder_ticks(double x);
public:
    double position;

private:
    //Initialization object
    //NameSpace for bridge controller
    ros::NodeHandle mNh;
    // Name of the motor
    string mMotorName;
    // Serial controller communication
    serial_controller *mSerial;
    // State of the motor
    double max_position;
    double velocity, max_velocity;
    double effort, max_effort;
    double command;

    int _control_mode;
    motor_status_t _status;

    joint_limits_interface::VelocityJointSoftLimitsInterface vel_limits_interface;

    // Publisher diagnostic information
    ros::Publisher pub_status, pub_control;
    // Message
    roboteq_control::MotorStatus msg_status;
    roboteq_control::ControlStatus msg_control;

    MotorParamConfigurator* parameter;
    MotorPIDConfigurator* pid_velocity;
    MotorPIDConfigurator* pid_torque;
    MotorPIDConfigurator* pid_position;

    GPIOSensor* _sensor;

    // Reader motor message
    void read(string data);

    void connectionCallback(const ros::SingleSubscriberPublisher& pub);
};

}

#endif // MOTOR_H