motor_board.hpp

Go to the documentation of this file.

#pragma once

#include <memory>
#include <string>

#include "real_time_tools/timer.hpp"
#include "real_time_tools/thread.hpp"

#include "blmc_drivers/utils/os_interface.hpp"
#include "real_time_tools/threadsafe/threadsafe_object.hpp"
#include "real_time_tools/threadsafe/threadsafe_timeseries.hpp"
#include "blmc_drivers/devices/can_bus.hpp"
#include "blmc_drivers/devices/device_interface.hpp"

namespace blmc_drivers
{

//==============================================================================
class MotorBoardCommand
{
public:

    MotorBoardCommand() { }

    MotorBoardCommand(uint32_t id, int32_t content)
    {
        id_ = id;
        content_ = content;
    }

    void print() const
    {
        rt_printf("command id: %d, content: %d\n", id_, content_);
    }

    enum IDs
    {
        ENABLE_SYS = 1,
        ENABLE_MTR1 = 2,
        ENABLE_MTR2 = 3,
        ENABLE_VSPRING1 = 4,
        ENABLE_VSPRING2 = 5,
        SEND_CURRENT = 12,
        SEND_POSITION = 13,
        SEND_VELOCITY = 14,
        SEND_ADC6 = 15,
        SEND_ENC_INDEX = 16,
        SEND_ALL = 20,
        SET_CAN_RECV_TIMEOUT = 30,
        ENABLE_POS_ROLLOVER_ERROR = 31,
    };

    enum Contents
    {
        ENABLE = 1,
        DISABLE = 0
    };

    uint32_t id_;

    int32_t content_;
};



//==============================================================================
class MotorBoardStatus
{
public:
    uint8_t system_enabled:1;

    uint8_t motor1_enabled:1;  // 1

    uint8_t motor1_ready:1;    // 2

    uint8_t motor2_enabled:1;  // 3

    uint8_t motor2_ready:1;    // 4

    uint8_t error_code:3;      // 5-7

    enum ErrorCodes
    {
        NONE = 0,
        ENCODER = 1,
        CAN_RECV_TIMEOUT = 2,
        CRIT_TEMP = 3,  // currently unused
        POSCONV = 4,
        POS_ROLLOVER = 5,
        OTHER = 7
    };

    void print() const
    {
        rt_printf("\tSystem enabled: %d\n", system_enabled);
        rt_printf("\tMotor 1 enabled: %d\n", motor1_enabled);
        rt_printf("\tMotor 1 ready: %d\n", motor1_ready);
        rt_printf("\tMotor 2 enabled: %d\n", motor2_enabled);
        rt_printf("\tMotor 2 ready: %d\n", motor2_ready);
        rt_printf("\tError Code: %d\n", error_code);
    }

    bool is_ready()
    {
        if(system_enabled &&
           motor1_enabled &&
           motor1_ready &&
           motor2_enabled &&
           motor2_ready &&
           !error_code)
        {
            return true;
        }
        else
        {
            return false;
        }
    }
};



//==============================================================================
class MotorBoardInterface: public DeviceInterface
{
public:
    virtual ~MotorBoardInterface() {}
    
    typedef real_time_tools::ThreadsafeTimeseries<double> ScalarTimeseries;
    typedef ScalarTimeseries::Index Index;
    typedef real_time_tools::ThreadsafeTimeseries<Index> IndexTimeseries;
    typedef real_time_tools::ThreadsafeTimeseries<MotorBoardStatus> StatusTimeseries;
    typedef real_time_tools::ThreadsafeTimeseries<MotorBoardCommand> CommandTimeseries;
    template<typename Type> using Ptr = std::shared_ptr<Type>;
    template<typename Type> using Vector = std::vector<Type>;

    enum MeasurementIndex {current_0, current_1,
                           position_0, position_1,
                           velocity_0, velocity_1,
                           analog_0, analog_1,
                           encoder_index_0, encoder_index_1,
                           measurement_count};

    enum ControlIndex {current_target_0, current_target_1, control_count};

    virtual Ptr<const ScalarTimeseries> get_measurement(
            const int& index) const = 0;

    virtual Ptr<const StatusTimeseries> get_status() const = 0;

    virtual Ptr<const ScalarTimeseries> get_control(const int& index) const = 0;

    virtual Ptr<const CommandTimeseries> get_command() const = 0;

    virtual Ptr<const ScalarTimeseries> get_sent_control(
            const int& index) const = 0;

    virtual Ptr<const CommandTimeseries> get_sent_command() const = 0;

    virtual void set_control(const double& control, const int& index) = 0;
    
    virtual void set_command(const MotorBoardCommand& command) = 0;

    virtual void send_if_input_changed() = 0;
};

template<typename Type>
std::vector<std::shared_ptr<Type> > create_vector_of_pointers(
  const size_t& size, const size_t& length)
{
    std::vector<std::shared_ptr<Type> > vector;
    vector.resize(size);
    for(size_t i = 0; i < size; i++)
    {
        vector[i] = std::make_shared<Type>(length);
    }
    return vector;
}

//==============================================================================
class CanBusMotorBoard: public  MotorBoardInterface
{
public:

    CanBusMotorBoard(std::shared_ptr<CanBusInterface> can_bus,
                     const size_t& history_length = 1000,
                     const int &control_timeout_ms = 100);

    ~CanBusMotorBoard();



    virtual Ptr<const ScalarTimeseries> get_measurement(const int& index) const
    {
        return measurement_[index];
    }

    virtual Ptr<const StatusTimeseries> get_status() const
    {
        return status_;
    }

    virtual Ptr<const ScalarTimeseries> get_control(const int& index) const
    {
        return control_[index];
    }

    virtual Ptr<const CommandTimeseries> get_command() const
    {
        return command_;
    }

    virtual Ptr<const ScalarTimeseries> get_sent_control(
            const int& index) const
    {
        return control_[index];
    }

    virtual Ptr<const CommandTimeseries> get_sent_command() const
    {
        return sent_command_;
    }

    virtual void set_control(const double& control, const int& index)
    {
        control_[index]->append(control);
    }

    virtual void set_command(const MotorBoardCommand& command)
    {
        command_->append(command);
    }

    virtual void send_if_input_changed();

    void wait_until_ready();

    bool is_ready();

    void pause_motors();

    void disable_can_recv_timeout();

private:

    template<typename T> int32_t bytes_to_int32(T bytes)
    {
        return (int32_t) bytes[3] + ((int32_t)bytes[2] << 8) +
                ((int32_t)bytes[1] << 16) + ((int32_t)bytes[0] << 24);
    }

    float q24_to_float(int32_t qval)
    {
        return ((float)qval / (1 << 24));
    }

    int32_t float_to_q24(float fval)
    {
        return ((int)(fval * (1 << 24)));
    }

    template<typename T> float qbytes_to_float(T qbytes)
    {
        return q24_to_float(bytes_to_int32(qbytes));
    }

    void send_newest_controls();

    void send_newest_command();

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

    void loop();

    void print_status();

private:  

    std::shared_ptr<CanBusInterface> can_bus_;

    enum CanframeIDs
    {
        COMMAND_ID= 0x00,
        IqRef     = 0x05,
        STATUSMSG = 0x10,
        Iq        = 0x20,
        POS       = 0x30,
        SPEED     = 0x40,
        ADC6      = 0x50,
        ENC_INDEX = 0x60
    };

    Vector<Ptr<ScalarTimeseries>> measurement_;

    Ptr<StatusTimeseries> status_;

    Vector<Ptr<ScalarTimeseries>> control_;

    Ptr<CommandTimeseries> command_;

    Vector<Ptr<ScalarTimeseries>> sent_control_;

    Ptr<CommandTimeseries> sent_command_;

    bool is_loop_active_;

    bool motors_are_paused_;


    int control_timeout_ms_;

    real_time_tools::RealTimeThread rt_thread_;

};

} // namespace blmc_drivers