/scout/motors/src/motors.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 "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

Also available in: Unified diff

Project

General

Profile

Colony » Colony Scout » Colony Scout OS

Revision 2814387f