brake.cc
Go to the documentation of this file.
00001 /*
00002  *  Copyright (C) 2005, 2007, 2009 Austin Robot Technology
00003  *
00004  *  License: Modified BSD Software License Agreement
00005  * 
00006  *  $Id: brake.cc 1161 2011-03-26 02:10:49Z jack.oquin $
00007  */
00008 
00016 #include <assert.h>
00017 #include <unistd.h>
00018 #include <stdio.h>
00019 #include <math.h>
00020 
00021 #include "ros/ros.h"
00022 
00023 #include <art/pid2.h>                   // PID control 
00024 
00025 #include <art_msgs/BrakeCommand.h>
00026 #include <art_msgs/BrakeState.h>
00027 
00028 #include <art_msgs/ArtHertz.h>
00029 
00030 
00031 #include "devbrake.h"                   // servo device interface
00032 
00080 #define NODE "brake"
00081 
00082 namespace
00083 {
00084   // .cfg variables:
00085   std::string port = "/dev/brake";      // tty port name
00086   bool  training = false;               // use training mode
00087   bool  diagnostic = false;             // enable diagnostic mode
00088   int   qDepth = 1;                     // ROS topic queue depths
00090 
00091   Pid   *pid;                           // PID control
00092   devbrake *dev;                        // servo device interface
00093   float brake_pos;                      // current brake position
00094   float set_point;                      // requested brake setting
00095 }
00096 
00097 // ROS topics used by this driver
00098 ros::Subscriber brake_cmd;              // /brake/cmd ROS topic
00099 ros::Publisher brake_state;             // /brake/state ROS topic
00100 
00101 int GetParameters(void)
00102 {
00103   // use private node handle to get parameters
00104   ros::NodeHandle mynh("~");
00105 
00106   mynh.getParam("port", port);
00107   ROS_INFO("brake port = %s", port.c_str());
00108 
00109   mynh.getParam("training", training);
00110   if (training)
00111     ROS_INFO("using training mode");
00112 
00113   mynh.getParam("diagnostic", diagnostic);
00114   if (diagnostic)
00115     ROS_INFO("using diagnostic mode");
00116 
00117   // allocate and initialize the devbrake interface
00118   dev = new devbrake(training);
00119 
00120   // Set brake parameters -- make sure the defaults won't strip the
00121   // servo hardware gears.  These values will be used for /dev/null,
00122   // in training mode, or in case of failure.  They may be updated by
00123   // devbrake when it detects calibration changes.
00124 
00125   // TODO: move these parameters into a subordinate class.
00126 
00127   if (!mynh.getParam("encoder_min", dev->encoder_min))
00128     dev->encoder_min = 0.0;
00129   if (!mynh.getParam("encoder_max", dev->encoder_max))
00130     dev->encoder_max = 50000.0;
00131   dev->encoder_range = dev->encoder_max - dev->encoder_min;
00132   ROS_INFO("configured encoder range [%.f, %.f]",
00133            dev->encoder_min, dev->encoder_max);
00134 
00135   if (!mynh.getParam("pot_off", dev->pot_off))
00136     dev->pot_off = 4.9;
00137   if (!mynh.getParam("pot_full", dev->pot_full))
00138     dev->pot_full = 0.49;
00139   dev->pot_range = dev->pot_full - dev->pot_off;
00140   ROS_INFO("configured potentiometer range [%.3f, %.3f]",
00141            dev->pot_off, dev->pot_full);
00142 
00143   if (!mynh.getParam("pressure_min", dev->pressure_min))
00144     dev->pressure_min = 0.85;
00145   if (!mynh.getParam("pressure_max", dev->pressure_max))
00146     dev->pressure_max = 4.5;
00147   dev->pressure_range = dev->pressure_max - dev->pressure_min;
00148   ROS_INFO("configured pressure range [%.3f, %.3f]",
00149            dev->pressure_min, dev->pressure_max);
00150 
00151   // allocate PID control and configure parameters
00152   pid = new Pid("pid", 0.25, 0.0, 0.7);
00153   pid->Configure(mynh);
00154 
00155   return 0;
00156 }
00157 
00158 // Set up the device.  Return 0 if things go well, and -1 otherwise.
00159 int Setup()
00160 {   
00161   int rc = dev->Open(port.c_str());
00162   if (rc != 0)
00163     {
00164       ROS_FATAL("device open failed: %d", rc);
00165       return -1;
00166     }
00167 
00168   ROS_INFO("device opened");
00169 
00170   // wherever dev->Open() left the brake becomes our initial set point
00171   set_point = brake_pos = dev->get_position();
00172 
00173   return 0;
00174 }
00175 
00176 // Shutdown the device
00177 int Shutdown()
00178 {
00179   dev->Close();
00180   ROS_INFO("device closed");
00181   return 0;
00182 }
00183 
00184 void ProcessCommand(const art_msgs::BrakeCommand::ConstPtr &cmd)
00185 {
00186   uint32_t request = cmd->request;
00187 
00188   // ignore all brake command messages when in training mode
00189   if (training)
00190     {
00191       ROS_DEBUG("in training mode: brake cmd %u ignored", request);
00192       return;
00193     }
00194 
00195   ROS_DEBUG("brake request %u, position %.3f", request, cmd->position);
00196 
00197   switch (request)
00198     {
00199     case art_msgs::BrakeCommand::Absolute:
00200       set_point = limit_travel(cmd->position);
00201       break;
00202     case art_msgs::BrakeCommand::Relative:
00203       set_point = limit_travel(brake_pos + cmd->position);
00204       break;
00205     default:
00206       {
00207         ROS_WARN("invalid brake request %u (ignored)", request);
00208         return;
00209       }
00210     }
00211 }
00212 
00213 // Poll device for current status.  Publish results as brake status.
00214 //
00215 // If an I/O fails, the corresponding values remain unchanged and old
00216 // data are published.  This is intentional.
00217 //
00218 float PollDevice(void)
00219 {
00220   art_msgs::BrakeState bs;             // brake state message
00221 
00222   // read the primary hardware sensor status
00223   dev->get_state(&bs.position, &bs.potentiometer, &bs.encoder, &bs.pressure);
00224 
00225 #if 0 // TODO: use ROS diagnostics package
00226   if (diagnostic)                       // return extra diagnostic values?
00227     {
00228       dev->query_amps(&aio_data.voltages[BrakeAmps]);
00229       dev->query_volts(&aio_data.voltages[BrakeVolts]);
00230       aio_data.voltages_count = BrakeDataMax;
00231     }
00232 #endif
00233 
00234   bs.header.stamp = ros::Time::now();
00235   brake_state.publish(bs);
00236 
00237   // return current position
00238   return bs.position;
00239 }
00240 
00241 
00242 int main(int argc, char** argv)
00243 {
00244   ros::init(argc, argv, NODE);
00245   ros::NodeHandle node;
00246 
00247   // topics to read and write
00248   brake_cmd = node.subscribe(NODE "/cmd", qDepth, ProcessCommand,
00249                              ros::TransportHints().tcpNoDelay(true));
00250   brake_state = node.advertise<art_msgs::BrakeState>(NODE "/state", qDepth);
00251 
00252   if (GetParameters() != 0)
00253     return 1;
00254 
00255   ros::Rate cycle(art_msgs::ArtHertz::BRAKE); // set driver cycle rate
00256 
00257   if (Setup() != 0)
00258     return 2;
00259 
00260   // Main loop; grab messages off our queue and republish them via ROS
00261   while(ros::ok())
00262     {
00263       brake_pos = PollDevice();         // get and publish device status
00264 
00265       ros::spinOnce();                  // handle incoming commands
00266 
00267       static float const epsilon = 0.001;
00268       float ctlout = pid->Update(set_point - brake_pos, brake_pos);
00269       if (fabs(ctlout) > epsilon)       // not quite close enough?
00270         dev->brake_relative(ctlout);
00271 
00272       cycle.sleep();                    // sleep until next cycle
00273     }
00274 
00275   Shutdown();
00276 
00277   return 0;
00278 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Defines


art_servo
Author(s): Austin Robot Technology, Jack O'Quin
autogenerated on Tue Sep 24 2013 10:43:22