zyonz_chlorophyll_meter_driver.cpp

Go to the documentation of this file.

#include "zyonz_chlorophyll_meter_driver.h"

ZyonzChlorophyllMeterDriver::ZyonzChlorophyllMeterDriver(void):
motor_(),
baudrate_(1000000),
bus_id_(0),
servo_id_(255),
max_angle_(-25),
min_angle_(-46),
effort_(10.0)
{
}

bool ZyonzChlorophyllMeterDriver::openDriver(void)
{
    std::string motor_name="chlorophill_meter_motor";

    std::cout << "opening ..." << std::endl;
    //setDriverId(driver string id);
    if(this->servo_id_==255)
    {
        ROS_INFO("No servo identifier has been set");
        return false;  
    }

    try
    {
        // configure the motor
        this->motor_ = boost::shared_ptr<CDynamixelMotor>(new CDynamixelMotor(motor_name, bus_id_, baudrate_, servo_id_));
        this->motor_->enable();
        this->motor_->set_max_torque(effort_);
        this->motor_->set_limit_torque(effort_);
    }
    catch(CDynamixelMotorException &e)
    {
        ROS_INFO("%s",e.what().c_str());
        return false;
    }

    ROS_INFO("Chlorophyll Meter is ready");

    TDynamixel_info info;
    motor_->get_servo_info(info);

    ROS_INFO("Motor Model  : %s", info.model.c_str());
    ROS_INFO("Firmware Ver.: %d", info.firmware_ver);
    ROS_INFO("Gear Ratio   : %d", info.gear_ratio);
    ROS_INFO("Encoder Res. : %d", info.encoder_resolution);
    ROS_INFO("PID Control  : %d", info.pid_control);
    ROS_INFO("Max. Angle   : %f", info.max_angle);
    ROS_INFO("Center Angle : %f", info.center_angle);
    ROS_INFO("Max. Speed   : %f", info.max_speed);
    ROS_INFO("Bus ID.      : %s", info.bus_id.c_str());
    ROS_INFO("Baudrate     : %u", info.baudrate);
    ROS_INFO("Servo ID.    : %d", info.id);

    return true;
}

bool ZyonzChlorophyllMeterDriver::closeDriver(void)
{
    std::cout << "closing ..." << std::endl;
    return true;
}

bool ZyonzChlorophyllMeterDriver::startDriver(void)
{
    std::cout << "starting ..." << std::endl;
    return true;
}

bool ZyonzChlorophyllMeterDriver::stopDriver(void)
{
    std::cout << "stoping ..." << std::endl;
    return true;
}

void ZyonzChlorophyllMeterDriver::config_update(Config& new_cfg, uint32_t level)
{
    this->lock();

    // depending on current state
    // update driver with new_cfg data
    switch(this->getState())
    {
        case ZyonzChlorophyllMeterDriver::CLOSED:
            // save the new values into the class
            this->servo_id_  = new_cfg.servo_id;
            this->bus_id_    = new_cfg.bus_id;
            this->baudrate_  = new_cfg.baudrate;
            this->max_angle_ = new_cfg.max_angle;
            this->min_angle_ = new_cfg.min_angle;
            this->effort_    = new_cfg.effort;
            break;

        case ZyonzChlorophyllMeterDriver::OPENED:
            break;

        case ZyonzChlorophyllMeterDriver::RUNNING:
            try{
                this->motor_->set_max_torque(new_cfg.effort);
                this->motor_->set_limit_torque(new_cfg.effort);
                // save the new values into the class
                this->max_angle_ = new_cfg.max_angle;
                this->min_angle_ = new_cfg.min_angle;
                this->effort_    = new_cfg.effort;
            }catch(CException &e){
                ROS_ERROR("%s",e.what().c_str());
                new_cfg.max_angle=this->max_angle_;
                new_cfg.min_angle=this->min_angle_;
                new_cfg.effort=this->effort_;
            }
            break;
    }

    // save the current configuration
    this->config_=new_cfg;

    this->unlock();
}

bool ZyonzChlorophyllMeterDriver::open_meter(void)
{
    return move_gripper(max_angle_);
}

bool ZyonzChlorophyllMeterDriver::close_meter(void)
{
    return move_gripper(min_angle_);
}

void ZyonzChlorophyllMeterDriver::stop_meter(void)
{
    if (this->getState() != ZyonzChlorophyllMeterDriver::CLOSED)
    {
        try
        {
            this->motor_->stop();
            this->motor_->disable();
        }
        catch (CDynamixelMotorException &e)
        {
            ROS_ERROR("%s", e.what().c_str());
        }
    }
}

float ZyonzChlorophyllMeterDriver::get_current_effort()
{
    return this->motor_->get_current_effort();
}

float ZyonzChlorophyllMeterDriver::get_max_effort(void)
{
    return this->effort_;
}

float ZyonzChlorophyllMeterDriver::get_current_angle(void)
{
    return this->motor_->get_current_angle();
}

float ZyonzChlorophyllMeterDriver::get_max_angle()
{
    return this->max_angle_;
}

float ZyonzChlorophyllMeterDriver::get_min_angle()
{
    return this->min_angle_;
}

bool ZyonzChlorophyllMeterDriver::is_at_max_angle()
{
    float error;
    error = abs(this->get_current_angle() - this->get_max_angle());
    return (error <= 5.0f);
}

bool ZyonzChlorophyllMeterDriver::is_at_min_angle()
{
    float error;
    error = abs(this->get_current_angle() - this->get_min_angle());
    return (error <= 5.0f);
}

void ZyonzChlorophyllMeterDriver::set_baudrate(int baudrate)
{
    this->baudrate_ = baudrate;
}

void ZyonzChlorophyllMeterDriver::set_bus_id(int bus_id)
{
    this->bus_id_ = bus_id;
}

void ZyonzChlorophyllMeterDriver::set_servo_id(unsigned char servo_id)
{
    this->servo_id_ = servo_id;
}

void ZyonzChlorophyllMeterDriver::set_max_angle(double max_angle)
{
    this->max_angle_ = max_angle;
}

void ZyonzChlorophyllMeterDriver::set_min_angle(double min_angle)
{
    this->min_angle_ = min_angle;
}

void ZyonzChlorophyllMeterDriver::set_effort(double effort)
{
    this->effort_ = effort;
}

bool ZyonzChlorophyllMeterDriver::move_gripper(const double position_in_radians)
{
    if (this->getState() == ZyonzChlorophyllMeterDriver::CLOSED) {
        ROS_WARN("[ZyonzChlorophyllMeterDriver::move_gripper] moving not possible -> driver is CLOSED");
        return false;
    }

    try
    {
        this->motor_->stop();
        ROS_INFO("Moving the gripper to: %f", position_in_radians);
        this->motor_->move_absolute_angle(position_in_radians, 100.0f);
    }
    catch (CDynamixelMotorException &e)
    {
        ROS_ERROR("%s", e.what().c_str());
        return false;
    }

    return true;
}

ZyonzChlorophyllMeterDriver::~ZyonzChlorophyllMeterDriver(void)
{
}