/scout/libscout/src/libmotors.cpp - Diff - Colony Scout OS - Robotics Club

Revision 2814387f

ID	2814387f48599dab8a99852093cc51629a14fb89

Added by Alex Zirbel over 12 years ago

Updated motors code.

Changed the definitions for the set_motors message, which forced a few
implementation changes as well.

Changed "libscout_node" to "node" and "n" to "node" in other packages.

     #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;
+    }

Also available in: Unified diff

Project

General

Profile

Colony » Colony Scout » Colony Scout OS

Revision 2814387f