hand.hpp

#pragma once
 
#include <iostream>
#include <vector>
#include <string>
#include <memory>
#include <cmath>
 
#include "dynamixel_sdk/dynamixel_sdk.h"
 
#include "colors.hpp"
 
#include "motor.hpp"
#include "fingermotor.hpp"
#include "wristmotor.hpp"
 
namespace uclv::dynamixel_utils
{
 
    class Hand
    {
    private:
        std::string serial_port_; 
        int baudrate_;            
        float protocol_version_;  
 
        dynamixel::PortHandler *portHandler_;     
        dynamixel::PacketHandler *packetHandler_; 
 
        std::vector<std::shared_ptr<FingerMotor>> fingerMotors_; 
        std::vector<std::shared_ptr<WristMotor>> wristMotors_;   
 
        std::unique_ptr<dynamixel::GroupSyncWrite> groupSyncWrite_ = nullptr; 
        std::unique_ptr<dynamixel::GroupSyncRead> groupSyncRead_ = nullptr;   
        std::unique_ptr<dynamixel::GroupBulkRead> groupBulkRead_ = nullptr;   
        std::unique_ptr<dynamixel::GroupBulkWrite> groupBulkWrite_ = nullptr; 
 
    public:
        Hand(const std::string &serial_port, int baudrate, float protocol_version,
             dynamixel::PortHandler *const &portHandler, dynamixel::PacketHandler *const &packetHandler);
 
        Hand(const std::string &serial_port, int baudrate, float protocol_version);
 
        Hand();
 
        ~Hand();
 
        bool initialize();
 
        void setSerialPort(const std::string &serial_port);
 
        void setBaudrate(int baudrate);
 
        void setProtocolVersion(float protocol_version);
 
        std::string getSerialPort();
 
        int getBaudrate();
 
        float getProtocolVersion();
 
        void setPortHandler(dynamixel::PortHandler *portHandler);
 
        void setPacketHandler(dynamixel::PacketHandler *packetHandler);
 
        void setSerialPortLowLatency(const std::string &serial_port);
 
        std::shared_ptr<FingerMotor> createFingerMotor(uint8_t id);
 
        std::shared_ptr<WristMotor> createWristMotor(uint8_t id);
 
        const std::vector<std::shared_ptr<FingerMotor>> &addFingerMotor(uint8_t id);
 
        const std::vector<std::shared_ptr<WristMotor>> &addWristMotor(uint8_t id);
 
        std::vector<std::shared_ptr<FingerMotor>> getFingerMotors();
 
        std::vector<std::shared_ptr<WristMotor>> getWristMotors();
 
        void printFingerMotors() const;
 
        void printWristMotors() const;
 
        void removeFingerMotor(uint8_t id);
 
        void removeWristMotor(uint8_t id);
 
        void moveFingerMotor(const uint8_t &id, const float &position);
 
        void moveWristMotor(const uint8_t &id, const float &position);
 
        uint16_t readFingerMotorPosition(const uint8_t &id);
 
        uint16_t readWristMotorPosition(const uint8_t &id);
 
        void moveMotors(const std::vector<uint8_t> &ids, const std::vector<float> &positions);
 
        std::vector<uint32_t> readMotorsPositions(const std::vector<uint8_t> &ids);
    };
}