motor.hpp

#pragma once
 
#include "dynamixel_sdk/dynamixel_sdk.h"
#include "colors.hpp"
#include <string>
 
namespace uclv::dynamixel_utils
{
 
    class Motor
    {
    private:
        uint8_t id_;              
        std::string serial_port_; 
        int baudrate_;            
        float protocol_version_;  
 
        uint16_t addrTargetPosition_ = 30;  
        uint16_t addrPresentPosition_ = 36; 
 
        uint16_t addrTorqueEnable_ = 24; 
 
        uint16_t lenAddrTargetPosition_ = 2;  
        uint16_t lenAddrPresentPosition_ = 2; 
 
        dynamixel::PortHandler *portHandler_;     
        dynamixel::PacketHandler *packetHandler_; 
 
    public:
        Motor(const std::string &serial_port, int baudrate, float protocol_version,
              dynamixel::PortHandler *const &portHandler, dynamixel::PacketHandler *const &packetHandler,
              uint8_t id);
 
        Motor(const std::string &serial_port, int baudrate, float protocol_version,
              uint8_t id);
 
        Motor();
 
        // DISTRUTTORE?
 
        void setId(uint8_t id);
 
        uint8_t getId();
 
        void setAddrTargetPosition(uint16_t addrTargetPosition);
 
        uint16_t getAddrTargetPosition() const;
 
        void setAddrPresentPosition(uint16_t addrPresentPosition);
 
        uint16_t getAddrPresentPosition() const;
 
        void setLenAddrTargetPosition(uint16_t lenAddrTargetPosition);
 
        uint16_t getLenAddrTargetPosition() const;
 
        void setLenAddrPresentPosition(uint16_t lenAddrPresentPosition);
 
        uint16_t getLenAddrPresentPosition() const;
 
        void setAddrTorqueEnable(uint16_t addrTorqueEnable);
 
        uint16_t getAddrTorqueEnable() const;
 
        void setTargetPosition(float position);
 
        uint16_t readPresentPosition();
 
        void write1OnAddress(uint8_t id, uint16_t address, uint8_t data);
 
        void write2OnAddress(uint8_t id, uint16_t address, uint16_t data);
 
        uint8_t read1FromAddress(uint8_t id, uint16_t address);
 
        uint16_t read2FromAddress(uint8_t id, uint16_t address);
 
        bool enableTorque();
 
        bool disableTorque();
    };
}