/ - Diff - Colony Scout OS - Robotics Club

     #include "ros/ros.h"
     /* ROS defines */
     #define QUEUE_SIZE 10
     /* Libscout Defines */
     /* Init defines */

     /* ROS node created in libscout.cpp */
     extern ros::NodeHandle libscout_node;
     extern ros::NodeHandle node;
     /** ROS publisher and client declaration */
     ros::Publisher button_event_publisher;
-...
      */
     void libbuttons_init()
+    {
       button_event_publisher = libscout_node.advertise<buttons::button_event>("libbuttons", 10);
       query_buttons_client = libscout_node.serviceClient<buttons::query_buttons>("libbuttons");
       button_event_publisher = node.advertise<buttons::button_event>("libbuttons", 10);
       query_buttons_client = node.serviceClient<buttons::query_buttons>("libbuttons");
+    }
     /*!

     #include "libheadlights.h"
     /* ROS node created in libscout.cpp */
     extern ros::NodeHandle libscout_node;
     extern ros::NodeHandle node;
     /** ROS publisher and client declaration */
     ros::Publisher set_headlights_publisher;
-...
     void libheadlights_init()
+    {
       set_headlights_publisher =
     		libscout_node.advertise<headlights::set_headlights>("libheadlights", 10);
     		node.advertise<headlights::set_headlights>("libheadlights", 10);
+    }
     /*!

     #include "libmotors.h"
     /* ROS node created in libscout.cpp */
     extern ros::NodeHandle libscout_node;
     extern ros::NodeHandle node;
     /** ROS publisher and client declaration */
     ros::Publisher set_motors_publisher;
     ros::ServiceClient query_motors_client;
     /*!
      * \brief Initialize the motors module of libscout.
     /**
      * @brief Initialize the motors module of libscout.
+     *
      * Initialize the libscout node as a publisher of set_motors and a client of
      * query_motors.
      */
     void libmotors_init()
+    {
       set_motors_publisher = libscout_node.advertise<motors::set_motors>("libmotors", 10);
       query_motors_client = libscout_node.serviceClient<motors::query_motors>("libmotors");
         set_motors_publisher =
             node.advertise<motors::set_motors>("set_motors", QUEUE_SIZE);
         query_motors_client =
             node.serviceClient<motors::query_motors>("query_motors");
+    }
     /*!
      * \brief Set motor speeds
     /**
      * @brief Sets all the speeds according to the specificiation of which motors
      *        to set.
+     *
      * @param which A bitmask of which motors need to be set.
      * @param speed The value to set the motors to.
      * @param units The units the speed is expressed in.
      * @return Function status
      */
     int motors_set(int which, float speed, char units)
+    {
         float speed_fl, speed_fr, speed_bl, speed_br;
         speed_fl = speed_fr = speed_bl = speed_br = speed;
         if(which & MOTOR_FL_REV)
+        {
             speed_fl = -speed;
+        }
         if(which & MOTOR_FR_REV)
+        {
             speed_fr = -speed;
+        }
         if(which & MOTOR_BL_REV)
+        {
             speed_bl = -speed;
+        }
         if(which & MOTOR_BR_REV)
+        {
             speed_br = -speed;
+        }
         return motors_set_each(which, speed_fl, speed_fr, speed_bl, speed_br,
                                units);
+    }
     /**
      * @brief Sets the left and right sides of scout to different speeds.
+     *
      * @param speed_l The speed of both left motors.
      * @param speed_r The speed of both right motors.
      * @param units The units to set to.
      * @return Function status
      */
     int motors_set_sides(float speed_l, float speed_r, char units)
+    {
         return motors_set_each(MOTOR_ALL, speed_l, speed_r, speed_l, speed_r,
                                    units);
+    }
     /**
      * @brief Set motor speeds
      * Sets the speeds of the motors as a percentage of top speed. Can selectively
      * select which motors to set, and which to remain at previous speed.
      * \param speed The speed the motors should be set to
      * \param which A bitmask of which motors should be set
      * \return Function status
+     *
      * @param which A bitmask of which motors should be set.
      * @param speed The speed the motors should be set to.
      * @param units Optional units for the speeds.
      * @return Function status
      */
     int motors_set(int speed, int which, char units=MOTOR_PERCENT)
     int motors_set_each(int which,
                         float speed_fl, float speed_fr,
                         float speed_bl, float speed_br,
                         char units)
+    {
       /** \todo Set fields of the message based on params */
     	motors::set_motors msg;
       if(!ros::ok())
+    	{
         return LIB_ERROR;
+      }
       /* Set the speed for each motor according to the which bitmask*/
       if(which & MOTOR_FL_REV)
+    	{
         msg.fl_speed = -1 * speed;
+      }
       if(which & MOTOR_FR_REV)
+    	{
         msg.fr_speed = -1 * speed;
+      }
       if(which & MOTOR_BL_REV)
+    	{
         msg.bl_speed = -1 * speed;
+      }
       if(which & MOTOR_BR_REV)
+    	{
         msg.br_speed = -1 * speed;
       }if(which & MOTOR_FL)
+    	{
         msg.fl_speed = speed;
+      }
       if(which & MOTOR_FR)
+    	{
         msg.fr_speed = speed;
+      }
       if(which & MOTOR_BL)
+    	{
         msg.bl_speed = speed;
+      }
       if(which & MOTOR_BR)
+    	{
         msg.br_speed = speed;
+      }
       /* Specify which units the speeds are given in */
       msg.units = units;
       /* Publishes message to set_motors topic */
       set_motors_publisher.publish(msg);
       ros::spinOnce();
       return LIB_OK;
         check_which_ok(which);
         motors::set_motors msg;
         if(!ros::ok())
+        {
             return LIB_ERROR;
+        }
         /* Set all the speeds (the booleans in the set_motors message determine
          * which ones will be used) */
         msg.fl_speed = speed_fl;
         msg.fr_speed = speed_fr;
         msg.bl_speed = speed_bl;
         msg.br_speed = speed_br;
         /* Tell the motors node which motors need to be updated */
         msg.fl_set = msg.fr_set = msg.bl_set = msg.br_set = false;
         if(which & (MOTOR_FL | MOTOR_FL_REV))
+        {
             msg.fl_set = true;
+        }
         if(which & (MOTOR_FR | MOTOR_FR_REV))
+        {
             msg.fr_set = true;
+        }
         if(which & (MOTOR_BL | MOTOR_BL_REV))
+        {
             msg.bl_set = true;
+        }
         if(which & (MOTOR_BR | MOTOR_BR_REV))
+        {
             msg.br_set = true;
+        }
         /* Specify which units the speeds are given in */
         msg.units = units;
         /* Publishes message to set_motors topic */
         set_motors_publisher.publish(msg);
         ros::spinOnce();
         return LIB_OK;
+    }
     /*!
      * \brief Query the current speeds of the motors
     /**
      * Double-checks the which variable to make sure the front and back
      * bits are not both set for a single motor.
+     *
      * @param which The which motor specfication to check.
      */
     void check_which_ok(int which)
+    {
         ROS_WARN_COND( ((which & MOTOR_FL) && (which & MOTOR_FL_REV)),
             "FL Motor set in both directions!");
         ROS_WARN_COND( ((which & MOTOR_FR) && (which & MOTOR_FR_REV)),
             "FR Motor set in both directions!");
         ROS_WARN_COND( ((which & MOTOR_BL) && (which & MOTOR_BL_REV)),
             "BL Motor set in both directions!");
         ROS_WARN_COND( ((which & MOTOR_BR) && (which & MOTOR_BR_REV)),
             "BR Motor set in both directions!");
+    }
     /**
      * @brief Query the current speeds of the motors
+     *
      * Sends a request to the query_motors service which will reply with the
      *  current speed of each motor.
+     *
      * \param which A bitmask that will specify which motor speed should be
      * @todo Change so we can get multiple motor speeds with one call
+     *
      * @param which A bitmask that will specify which motor speed should be
      *  returned
      * \return The speed of the selected motor, or LIB_ERR if no motor selected
      * @return The speed of the selected motor, or LIB_ERR if no motor selected
      */
     int motors_query(int which)
     float motors_query(int which)
+    {
       motors::query_motors srv;
       if(query_motors_client.call(srv))
+    	{
         switch(which)
+    		{
           case MOTOR_FL:
             return srv.response.fl_speed;
           case MOTOR_FR:
             return srv.response.fr_speed;
           case MOTOR_BL:
             return srv.response.bl_speed;
           case MOTOR_BR:
             return srv.response.br_speed;
           default:
             ROS_WARN("Bad WHICH in motors_query.");
         motors::query_motors srv;
         if(query_motors_client.call(srv))
+        {
             switch(which)
+            {
                 case MOTOR_FL:
                     return srv.response.fl_speed;
                 case MOTOR_FR:
                     return srv.response.fr_speed;
                 case MOTOR_BL:
                     return srv.response.bl_speed;
                 case MOTOR_BR:
                     return srv.response.br_speed;
                 default:
                     ROS_WARN("Bad WHICH in motors_query.");
                     return LIB_ERROR;
+            }
+        }
         else
+        {
             ROS_ERROR("Failed to call service query_motors");
             return LIB_ERROR;
+        }
+      }
     	else
+    	{
         ROS_ERROR("Failed to call service query_motors");
         return LIB_ERROR;
+      }
       return 0;
         return 0;
+    }

     #define MOTOR_RIGHT_REV MOTOR_FR_REV | MOTOR_BR_REV
     #define MOTOR_LEFT_SPIN MOTOR_LEFT_REV | MOTOR_RIGHT
     #define MOTOR_RIGHT_SPIN MOTOR_LEFT | MOTOR_RIGHT_REV
     #define MOTOR_PERCENT 'p'
     #define MOTOR_MMS 'm'
     #define MOTOR_CMS 'c'
     #define MOTOR_ABSOLUTE 'a'
     #define MOTOR_DEFAULT_UNIT MOTOR_PERCENT
     void libmotors_init();
     int motors_set(int speed, int which);
     int motors_query(int which);
     int motors_set(int which, float speed, char units=MOTOR_DEFAULT_UNIT);
     int motors_set_sides(float speed_l, float speed_r,
                          char units=MOTOR_DEFAULT_UNIT);
     int motors_set_each(int which,
                         float speed_fl, float speed_fr,
                         float speed_bl, float speed_br,
                         char units=MOTOR_DEFAULT_UNIT);
     void check_which_ok(int which);
     float motors_query(int which);
     #endif

     #include "libscout.h"
     /* Global objects */
     ros::NodeHandle libscout_node;
     ros::NodeHandle node;
     /** \todo Decide how to call user behaviors
       * I'm thinking that there be a function call in main that calls the "main"

     _LIBSCOUT_H_	libscout.h	41
     init	libscout.cpp	/^int init(int modules){$/
     libmotors_init	libmotors.cpp	/^void libmotors_init(){$/
     libscout_node	constants.h	58
     node	constants.h	58
     motors_query	libmotors.cpp	/^int motors_query(char which){$/
     motors_set	libmotors.cpp	/^void motors_set(int speed, char which){$/
     motors_speed_set	libmotors.cpp	/^void motors_speed_set(int speed, char which){$/

          motors
          Module to drive the motors and provide all motor control functionality.
       </description>
       <author>Ben</author>
       <author>Ben Wasserman</author>
       <license>BSD</license>
       <review status="unreviewed" notes=""/>
       <url>https://www.roboticsclub.org/redmine/projects/colonyscout/wiki</url>
       <depend package="roscpp"/>
       <depend package="std_msgs"/>
       <export>
         <cpp cflags="-I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lmotors"/>
       </export>
     </package>

     Header header
     int8 fl_speed
     int8 fr_speed
     int8 bl_speed
     int8 br_speed
     # Keeps track of which motors need to be changed
     int8 which
     # Set these to true if the motor should be updated
     bool fl_set
     bool fr_set
     bool bl_set
     bool br_set
     # The motor speeds, from -255 to 255 if absolute
     # -100 to 100 if percentage
     # Decimal values if in measured units
     float32 fl_speed
     float32 fr_speed
     float32 bl_speed
     float32 br_speed
     # Allows motor speeds to be specified mm/s, cm/s, percent, or absolute
     int8 units

      **/
     #include "ros/ros.h"
     #include "motors_util.cpp"
     #include "motors.h"
     //#include "libscout/src/constants.h"
     #include <cstdlib>
-...
      * @{
      **/
     /* Motor state variables
      * Speeds expressed as absolute speed out of max speed (0 - +-MAXSPEED)
      * Absolute speed is the speed written to the hardware to move the motors
      */
     /** @todo Fix types: static */
     int motor_fl_speed; /**< The current speed of the front left motor. */
     int motor_fr_speed; /**< The current speed of the front right motor. */
     int motor_bl_speed; /**< The current speed of the back left motor. */
     int motor_br_speed; /**< The current speed of the back right motor. */
     static int motor_fl_speed; /**< The current speed of the front left motor. */
     static int motor_fr_speed; /**< The current speed of the front right motor. */
     static int motor_bl_speed; /**< The current speed of the back left motor. */
     static int motor_br_speed; /**< The current speed of the back right motor. */
     /**
      * @brief Sets motor speed
-...
      */
     void motors_set(const motors::set_motors::ConstPtr& msg)
+    {
         /** @todo Edit to only set requested motors, not all */
         int which = msg->which;
         if(which & MOTOR_FL_REV)
+        {
           motor_fl_speed = -1 * motors_rel_to_abs(msg->fl_speed, msg->units);
+        }
         if(which & MOTOR_FR_REV)
+        {
           motor_fr_speed = -1 * motors_rel_to_abs(msg->fr_speed, msg->units);
+        }
         if(which & MOTOR_BL_REV)
+        {
           motor_bl_speed = -1 * motors_rel_to_abs(msg->bl_speed, msg->units);
+        }
         if(which & MOTOR_BR_REV)
+        {
           motor_br_speed = -1 * motors_rel_to_abs(msg->br_speed, msg->units);
+        }
         if(which & MOTOR_FL)
         if(msg->fl_set)
+        {
           motor_fl_speed = motors_rel_to_abs(msg->fl_speed, msg->units);
+        }
         if(which & MOTOR_FR)
         if(msg->fr_set)
+        {
           motor_fr_speed = motors_rel_to_abs(msg->fr_speed, msg->units);
+        }
         if(which & MOTOR_BL)
         if(msg->bl_set)
+        {
           motor_bl_speed = motors_rel_to_abs(msg->bl_speed, msg->units);
+        }
         if(which & MOTOR_BR)
         if(msg->br_set)
+        {
           motor_br_speed = motors_rel_to_abs(msg->br_speed, msg->units);
+        }
         /* Write speeds to hardware */
         /** @todo Add code to write speeds to hardware */
         ROS_DEBUG("Motor speeds set");
+    }
     /**
-...
+    }
     /**
      * @brief Converts set speeds (of various units) to absolute speeds.
+     *
      * @param speed The speed expressed in the desired units
      * @param units The units the desired speed is measured in
      * @return The absolute speed of the motor
      **/
     int motors_rel_to_abs(int rel_speed, int units)
+    {
         switch(units)
+        {
           case MOTOR_ABSOLUTE:/* Speed given as absolute */
             return rel_speed;
           case MOTOR_PERCENT:/* Convert from percentage */
             return rel_speed * MAXSPEED / 100;
           case MOTOR_MMS:/* Convert from mm/s */
             /** @todo Make math to do this conversion **/
             return rel_speed;
           case MOTOR_CMS:/* Convert from cm/s */
             /** @todo Make math to do this conversion **/
             return rel_speed;
           default: /* The units aren't recognized */
             /** @todo Decide on default case. Either percent or absolute. **/
             return rel_speed;
+        }
+    }
     /**
      * @brief Convert absolute speeds to speeds of various units.
+     *
      * @param speed The speed expressed in absolute units.
      * @param units The units the desired speed is measured in.
      * @return The relative speed of the motor in desired units.
      **/
     int motors_abs_to_rel(int abs_speed, int units)
+    {
         switch(units)
+        {
           case MOTOR_ABSOLUTE:/* Speed given as absolute */
             return abs_speed;
           case MOTOR_PERCENT:/* Convert from percentage */
             return abs_speed * 100 / MAXSPEED;
           case MOTOR_MMS:/* Convert from mm/s */
             /** @todo Make math to do this conversion **/
             return abs_speed;
           case MOTOR_CMS:/* Convert from cm/s */
             /** @todo Make math to do this conversion **/
             return abs_speed;
           default: /* The units aren't recognized */
             /** @todo Decide on default case. Either percent or absolute. **/
             return abs_speed;
+        }
+    }
     /**
      * @brief Motors driver. This is a ROS node that controls motor speeds.
+     *
      * This is the main function for the motors node. It is run when the node
-...
         ros::init(argc, argv, "motors_driver");
         /* Advertise that this serves the query_motors service */
         ros::NodeHandle n;
         ros::ServiceServer service = n.advertiseService("query_motors",
                                                         motors_query);
         ros::NodeHandle node;
         ros::ServiceServer service = node.advertiseService("query_motors",
                                                            motors_query);
         /* Subscribe to the set_motors topic */
         ros::Subscriber sub0 = n.subscribe("set_motors", QUEUE_SIZE, motors_set);
         ros::Subscriber sub0 = node.subscribe("set_motors", QUEUE_SIZE, motors_set);
         /* Initialize hardware for motors */
         // Hardware init functions here

     #include "motors/query_motors.h"
     #include "motors/set_motors.h"
     #define MAXSPEED 512
     #define MOTOR_ALL 0xF
     #define MOTOR_ALL_REV 0xF0
     #define MOTOR_NONE 0x0
     #define MOTOR_FL 0x8
     #define MOTOR_FR 0x4
     #define MOTOR_BL 0x2
     #define MOTOR_BR 0x1
     #define MOTOR_FL_REV 0x80
     #define MOTOR_FR_REV 0x40
     #define MOTOR_BL_REV 0x20
     #define MOTOR_BR_REV 0x10
     #define MOTOR_FRONT MOTOR_FL | MOTOR_FR
     #define MOTOR_BACK MOTOR_BR | MOTOR_BR
     #define MOTOR_LEFT MOTOR_FL | MOTOR_BL
     #define MOTOR_RIGHT MOTOR_FR | MOTOR_BR
     #define MOTOR_LEFT_REV MOTOR_FL_REV | MOTOR_BL_REV
     #define MOTOR_RIGHT_REV MOTOR_FR_REV | MOTOR_BR_REV
     #define MOTOR_LEFT_SPIN MOTOR_LEFT_REV | MOTOR_RIGHT
     #define MOTOR_RIGHT_SPIN MOTOR_LEFT | MOTOR_RIGHT_REV
     #define MAXSPEED 255
     #define MOTOR_PERCENT 'p'
     #define MOTOR_MMS 'm'
     #define MOTOR_CMS 'c'

Project

General

Profile

Colony » Colony Scout » Colony Scout OS

Revision 2814387f