Colony » Colony Scout

/**

 * Copyright (c) 2011 Colony Project

 * 

 * Permission is hereby granted, free of charge, to any person

 * obtaining a copy of this software and associated documentation

 * files (the "Software"), to deal in the Software without

 * restriction, including without limitation the rights to use,

 * copy, modify, merge, publish, distribute, sublicense, and/or sell

 * copies of the Software, and to permit persons to whom the

 * Software is furnished to do so, subject to the following

 * conditions:

 * 

 * The above copyright notice and this permission notice shall be

 * included in all copies or substantial portions of the Software.

 * 

 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,

 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES

 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND

 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT

 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,

 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING

 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR

 * OTHER DEALINGS IN THE SOFTWARE.

 */

/**

 * @file encoders.cpp

 * @brief Encoders

 *

 * Implementation of functions for encoder use.

 *

 * @author Colony Project, CMU Robotics Club

 **/

#include "ros/ros.h"

#include "encoders.h"

//#include "libscout/src/constants.h"

#include <cstdlib>

/**

 * @defgroup encoders Encoders

 * @brief Functions for using the encoders

 *

 **/

/* Encoder state variables */

/** \todo Fix types: static */

/** \todo Fix integer overflow possibility */

/* The current position of each encoder, measured in encoder ticks */

int encoder_fl_index; /**< The current state of the front left encoder. */

int encoder_fr_index; /**< The current state of the front right encoder. */

int encoder_bl_index; /**< The current state of the back left encoder. */

int encoder_br_index; /**< The current state of the back right encoder. */

/* The current velocity of each encoder, measured in ticks per second */

int encoder_fl_velocity;

int encoder_fr_velocity;

int encoder_bl_velocity;

int encoder_br_velocity;

/* The current distance each encoder has traveled in ticks from last reset */

int encoder_fl_distance;

int encoder_fr_distance;

int encoder_bl_distance;

int encoder_br_distance;

/* The units that the encoder measurements will be published in */

char encoder_units;

/*!

 * \brief Encoders driver. This is a ROS node that controls encoders.

 *

 * This is the main function for the encoders node. It is run when the node

 * starts and initializes the encoders. It then publishes to the

 * encoder_state topic and advertises the query_encoders service.

 * 

 * \param argc The number of command line arguments (should be 1)

 * \param argv The array of command line arguments

 **/

int main(int argc, char **argv){

  /* Initialize in ROS the encoders driver node */

  ros::init(argc, argv, "encoders_driver");

  /* Advertise that this serves the query_encoders service */

  ros::NodeHandle n;

  ros::ServiceServer service = n.advertiseService("query_encoders", encoders_query);

  /* Subscribe to the encoder_state topic */

  ros::Subscriber sub0 = n.subscribe("encoder_state", 4, encoder_state);

  /* Initialize hardware for motors */

  // Hardware init functions here

  ROS_INFO("Ready to set encoders.");

  ros::spin();

  return 0;

}

/*!

 * \brief Gets encoder state

 *

 * Sets the encoder state based on subscription to the encoder_state topic.

 *

 * \todo Make this function set the state properly. Probably requires 

 * creating a new msg type for the mode set.

 *

 * \param msg The message from the encoder_state topic, containing encoder

 * configuration settings.

 */

void encoder_set_mode(const encoders::encoder_state::ConstPtr& msg)

{ 

}

/*!

 * \brief Publishes encoder state

 *

 * Publishes the current encoders position, velocty, and distance from last

 * reset.

 */

void encoders_state_publish()

{

        encoders::encoders_state msg;

        if(!ros::ok())

        {

    return LIB_ERROR;

  }

        /* Put index, velocity, and distance data in message */

        msg.fl_index = encoders_ticks_to_units(encoder_fl_index, encoder_units);

        msg.fr_index = encoders_ticks_to_units(encoder_fr_index, encoder_units);

        msg.bl_index = encoders_ticks_to_units(encoder_bl_index, encoder_units);

        msg.br_index = encoders_ticks_to_units(encoder_br_index, encoder_units);

        msg.fl_speed = encoders_ticks_to_units(encoder_fl_velocity, encoder_units);

        msg.fr_speed = encoders_ticks_to_units(encoder_fr_velocity, encoder_units);

        msg.bl_speed = encoders_ticks_to_units(encoder_bl_velocity, encoder_units);

        msg.br_speed = encoders_ticks_to_units(encoder_br_velocity, encoder_units);

        msg.fl_distance = encoders_ticks_to_units(encoder_fl_distance, encoder_units);

        msg.fr_distance = encoders_ticks_to_units(encoder_fr_distance, encoder_units);

        msg.bl_distance = encoders_ticks_to_units(encoder_bl_distance, encoder_units);

        msg.br_distance = encoders_ticks_to_units(encoder_br_distance, encoder_units);

        /* Specifies which units the encoder values are given in */

        msg.units = encoder_units;

        /* Publishes message to encoders_state */

        encoders_state_publisher.publish(msg);

        ros::spinOnce();

        return LIB_OK;

}        

/*!

 * \brief Outputs current encoder data

 *

 * Serves the service query_encoders by responding to service requests with the

 * encoder values.

 * \param req The request. The units that the response should be in.

 * \param res The response. The fields will be filled with values.

 */

bool encoders_query(encoders::query_encoders::Request &req,

    encoders::query_encoders::Response &res)

{

  int units = req.units;        

  /* Put index, velocity, and distance data in message */

        res.fl_index = encoders_ticks_to_units(encoder_fl_index, units);

        res.fr_index = encoders_ticks_to_units(encoder_fr_index, units);

        res.bl_index = encoders_ticks_to_units(encoder_bl_index, units);

        res.br_index = encoders_ticks_to_units(encoder_br_index, units);

        res.fl_speed = encoders_ticks_to_units(encoder_fl_velocity, units);

        res.fr_speed = encoders_ticks_to_units(encoder_fr_velocity, units);

        res.bl_speed = encoders_ticks_to_units(encoder_bl_velocity, units);

        res.br_speed = encoders_ticks_to_units(encoder_br_velocity, units);

        res.fl_distance = encoders_ticks_to_units(encoder_fl_distance, units);

        res.fr_distance = encoders_ticks_to_units(encoder_fr_distance, units);

        res.bl_distance = encoders_ticks_to_units(encoder_bl_distance, units);

        res.br_distance = encoders_ticks_to_units(encoder_br_distance, units);

  ROS_DEBUG("Encoder values queried");

  return true;

}

int encoders_ticks_to_units(int ticks, char units)

{

        switch(encoder_units)

        {

                case ENCODER_TICKS: /* Just return ticks */

                        return ticks;

                case ENCODER_MM: /* Return mm of linear travel by the wheel */

                        return ticks * (2 * PI * WHEEL_RADIUS_MM / NUM_TICKS);

                case ENCODER_CM: /* Return cm of linear travel by the wheel */

                        return ticks * (2 * PI * WHEEL_RADIUS_MM / NUM_TICKS) / 10;

                case ENCODER_DEG: /* Return degrees of travel from last reset */

                        return ticks * (360 / NUM_TICKS);

                default: /* The units aren't recognized. Return ticks. */

                        return ticks;

        }

}