devbrake.cc
Go to the documentation of this file.
00001 /*
00002  *  ART brake servo controller device interface
00003  *
00004  *  Copyright (C) 2005, 2009 Austin Robot Technology, Jack O'Quin
00005  *
00006  *  License: Modified BSD Software License Agreement
00007  *
00008  *  $Id: devbrake.cc 1876 2011-11-26 23:48:08Z jack.oquin $
00009  */
00010 
00011 #include <assert.h>
00012 #include <errno.h>
00013 #include <stdlib.h>
00014 #include <unistd.h>
00015 #include <string.h>
00016 #include <fcntl.h>
00017 #include <termios.h>
00018 #include <poll.h>
00019 
00020 #include <ros/ros.h>
00021 
00022 #include <art/conversions.h>
00023 
00024 #include "devbrake.h"
00025 
00026 
00028 // public methods
00030 
00032 // Get the time (in ms)
00033 //
00034 int64_t GetTime()
00035 {
00036   ros::Time tv = ros::Time::now();
00037   return (int64_t) tv.sec * 1000 + (int64_t) tv.nsec / 1000000;
00038 }
00039 
00040 
00041 
00042 devbrake::devbrake(bool train)
00043 {
00044   training = train;
00045   already_configured = false;
00046   cur_position = 0.0;
00047   prev_pressure = 0.0;
00048   sim = NULL;
00049 
00050   // use private node handle to get parameters
00051   ros::NodeHandle mynh("~");
00052   mynh.param("apply_on_exit", apply_on_exit, false);
00053 
00054   mynh.param("pressure_filter_gain", pressure_filter_gain, 0.4);
00055   ROS_INFO("brake pressure RC filter gain is %.3f", pressure_filter_gain);
00056 
00057   mynh.param("use_pressure", use_pressure, true);
00058   ROS_INFO("use %s sensor to control brake",
00059           (use_pressure? "pressure": "position"));
00060 
00061   // A deceleration_limit of 0.025 corresponds to approximately
00062   // 20Hz * 2250 (=0.025*90000) tics/cycle = 45,000 tics/sec.
00063   //
00064   // Assuming a typical encoder range of 0-90,000 tics and a 
00065   // threshold of 0.7 (= 63,000 tics), 0.6 seconds are
00066   // required for the final 30% of brake travel for full brake.
00067   //
00068   // At least one brake motor has failed repeatedly at limit=0.04
00069   // and once out of 100 attempts at limit=0.015. Replaced motor.
00070 
00071   mynh.param("deceleration_threshold", deceleration_threshold, 0.7);
00072   mynh.param("deceleration_limit", deceleration_limit, 0.025);
00073   ROS_INFO("brake deceleration threshold, limit is [%.3f, %.3f]",
00074            deceleration_threshold, deceleration_limit);
00075 }
00076 
00077 devbrake::~devbrake()
00078 {
00079   if (sim)
00080     delete sim;
00081 };
00082 
00083 int devbrake::Open(const char *port_name)
00084 {
00085   // retry count in case calibration fails
00086   int retries = 7;
00087 
00088   // open the serial port
00089   int rc = this->Servo::Open(port_name, (O_RDWR|O_NOCTTY|O_NONBLOCK));
00090   if (fd < 0) {
00091     ROS_FATAL("Couldn't open %s (%s)", port_name, strerror(errno));
00092     return -1;
00093   }  
00094 
00095   // set initial baud rate
00096   rc = configure_raw_port(B9600 | CS8, 0);
00097   if (rc != 0) goto fail;
00098     
00099   // tell device to run at 38400; command does not and cannot respond
00100   servo_write_only("BAUD38400\n");
00101   sleep(3);
00102     
00103   // set actual baud rate
00104   rc = configure_raw_port(B38400 | CS8, 0);
00105   if (rc != 0) goto fail;
00106 
00107   // Initialize brake simulation before calibration.
00108   if (!have_tty)
00109     sim = new ArtBrakeModel(cur_position);
00110 
00111   // No need to configure or calibrate brake if already done.
00112   // Must avoid touching the brake when in training mode.
00113   while (!already_configured && !training)
00114     {
00115       rc = configure_brake();
00116       if (rc != 0) goto fail;
00117 
00118       // For some reason (probably clutch slippage) calibrate brake
00119       // sometimes fails, yet retrying may succeed.
00120       rc = calibrate_brake();
00121       if (rc == 0)
00122         already_configured = true;
00123       else if (--retries <= 0)
00124         goto fail;
00125     }
00126 
00127   // set brake on unless in training mode
00128   if (!training)
00129     rc = brake_absolute(1.0);           // set brake fully on
00130 
00131   return rc;
00132 
00133  fail:
00134   this->Servo::Close();
00135   return rc;                            // Open() failed
00136 }
00137 
00138 int devbrake::Close()
00139 {
00140   if (apply_on_exit && !training)
00141     {
00142       ROS_INFO("setting brake fully on for shutdown");
00143       brake_absolute(1.0);              // set brake fully on
00144       sleep(1);                         // give it a second to work
00145     }
00146   return this->Servo::Close();
00147 }
00148 
00149 int devbrake::brake_absolute(float position)
00150 {
00151   ROS_DEBUG("brake_absolute(%.3f)", position);
00152   return brake_relative(position - cur_position);
00153 }
00154  
00155 int devbrake::brake_relative(float delta)
00156 {
00157   ROS_DEBUG("brake_relative(%.3f)", delta);
00158 
00159   float new_pos = limit_travel(cur_position + delta);
00160   if ((delta > 0.0) && (new_pos > deceleration_threshold))
00161     {
00162       // limit rate of deceleration, near full brake, to minimize
00163       // brake overcurrent errors
00164       bool limiter_engaged = true;
00165       if (cur_position < deceleration_threshold)
00166         // threshold crossing. Take minimum of new_pos and (threshold
00167         // + limit)
00168         new_pos = limit_travel(fminf(new_pos,
00169                                      deceleration_threshold
00170                                      + deceleration_limit));
00171       else if (delta > deceleration_limit)
00172         // both new and old position beyond threshold AND requested
00173         // delta greater than limit. Use limit
00174         new_pos = limit_travel(cur_position + deceleration_limit);
00175       else
00176         limiter_engaged = false;
00177 
00178       if (limiter_engaged)
00179         ROS_DEBUG("deceleration limiter engaged. cur_position: %.3f "
00180                   "requested delta: %.3f actual delta: %.3f",
00181                   cur_position, delta, new_pos-cur_position);
00182     }
00183 
00184   ROS_DEBUG("changing brake position to %.3f", new_pos);
00185   return encoder_goto((int) rintf((new_pos-cur_position)*encoder_range));
00186 }
00187 
00189 //
00190 //  Called once per cycle before other commands.
00191 //
00192 //  \returns 0, if all sensors are read successfully;
00193 //           errno value of last sensor that returned an error otherwise.
00194 int devbrake::get_state(float *position, float *potentiometer,
00195                         float *encoder, float *pressure)
00196 {
00197   // Read the primary hardware sensors, set rc to either zero or the
00198   // last error return code.  The query methods only modify their data
00199   // parameter if successful.
00200   int rc = query_encoder(encoder);
00201   int qrc = query_pot(potentiometer);
00202   if (qrc) rc = qrc;
00203   qrc = query_pressure(pressure);
00204   if (qrc) rc = qrc;
00205 
00206   // cur_position was set as a side-effect of the queries above
00207   *position = cur_position;
00208 
00209   return rc;
00210 }
00211 
00212 int devbrake::query_amps(float *data)
00213 {
00214   char string[MAX_SERVO_CMD_BUFFER];
00215 
00216   int rc = query_cmd("c=UIA Rc\n", string, MAX_SERVO_CMD_BUFFER);
00217   if (rc == 0)                  // only update if successful
00218     {
00219       *data = strtof(string, NULL);
00220       // TODO: convert *data to a voltage
00221       ROS_DEBUG("query amperage returns %.1f", *data);
00222     }
00223 
00224   return rc;
00225 }
00226 
00227 int devbrake::query_encoder(float *data)
00228 {
00229   char string[MAX_SERVO_CMD_BUFFER];
00230 
00231   int rc = query_cmd("RP\n", string, MAX_SERVO_CMD_BUFFER);
00232   if (rc == 0)                  // only update if successful
00233     {
00234       *data = cur_encoder = strtof(string, NULL);
00235       check_encoder_limits();           // adjust limits if motor slipped
00236       ROS_DEBUG("query encoder returns %.1f", *data);
00237     }
00238 
00239   return rc;
00240 }
00241 
00242 int devbrake::query_pot(float *data)
00243 {
00244   char string[MAX_SERVO_CMD_BUFFER];
00245 
00246   int rc = query_cmd("c=UEA Rc\n", string, MAX_SERVO_CMD_BUFFER);
00247   if (rc == 0)                          // only update if successful
00248     {
00249       // A/D value of potentiometer
00250       float pot_val = strtof(string, NULL);
00251 
00252       // convert A/D input to voltage (10-bit converter with 5-volt range)
00253       *data = cur_pot = analog_volts(pot_val, 5.0, 10);
00254       ROS_DEBUG("brake potentiometer voltage is %.3f (A/D %.f)",
00255                 cur_pot, pot_val);
00256 
00257       if (!use_pressure && already_configured)
00258         // use potentiometer to estimate current position
00259         cur_position = limit_travel(pot2pos(cur_pot));
00260     }
00261 
00262   return rc;
00263 }
00264 
00265 // RC filter transfer function: y[k] = (1-a)*x[k] + a*y[k-1]
00266 static inline float RC_filter(float a, float xk, float yk1)
00267 {
00268   return (1-a)*xk + a*yk1;
00269 }
00270 
00271 int devbrake::query_pressure(float *data)
00272 {
00273   char string[MAX_SERVO_CMD_BUFFER];
00274 
00275   int rc = query_cmd("c=UAA Rc\n", string, MAX_SERVO_CMD_BUFFER);
00276   if (rc == 0)                  // only update if successful
00277     {
00278       // A/D value of pressure sensor
00279       float pressure_val = strtof(string, NULL);
00280 
00281       // convert A/D input to voltage (10-bit converter with 5-volt range)
00282       cur_pressure = analog_volts(pressure_val, 5.0, 10);
00283 
00284       // smooth the data using an RC low-pass filter to eliminate
00285       // small fluctuations.
00286       cur_pressure = RC_filter(pressure_filter_gain,
00287                                cur_pressure, prev_pressure);
00288       ROS_DEBUG("RC filter output = %.3f, previous = %.3f",
00289                 cur_pressure, prev_pressure);
00290 
00291       ROS_DEBUG("brake pressure voltage is %.3f (A/D %.f)",
00292                 cur_pressure, pressure_val);
00293       *data = cur_pressure;             // return result to caller
00294 
00295       if (use_pressure && already_configured)
00296         // use pressure to estimate current position
00297         cur_position = limit_travel(press2pos(cur_pressure));
00298 
00299       prev_pressure = cur_pressure;
00300     }
00301 
00302   return rc;
00303 }
00304 
00305 int devbrake::query_volts(float *data)
00306 {
00307   char string[MAX_SERVO_CMD_BUFFER];
00308 
00309   int rc = query_cmd("c=UJA Rc\n", string, MAX_SERVO_CMD_BUFFER);
00310   if (rc == 0)                  // only update if successful
00311     {
00312       *data = strtof(string, NULL);
00313       // TODO: convert *data to a voltage
00314       ROS_DEBUG("query voltage returns %.3f", *data);
00315     }
00316 
00317   return rc;
00318 }
00319 
00320 
00322 // private methods
00324 
00325 
00326 #define ENC_EPSILON 2                   // trivial position difference
00327 #define POT_EPSILON 0.002               // trivial potentiometer difference
00328 #define PRESS_EPSILON 0.005             // trivial pressure difference
00329 
00330 // calibrate brake position limits
00331 //
00332 //  On entry, brake is off; on exit fully engaged.
00333 //
00334 //  Use potentiometer to estimate position, determining experimentally
00335 //  the achievable full brake limit.  Make this result available to
00336 //  translate potentiometer values to fractional position information.
00337 //
00338 // returns: 0 if successful, updates limits and ranges for encoder,
00339 //                           potentiometer, and pressure
00340 //          nonzero if failure, causing the setup() to fail
00341 //
00342 //   Experiments suggest that the brake is off when the pressure
00343 //   sensor is below 1.001v.  The corresponding potentiometer sensor
00344 //   value is around 3.8v.  Full brake is about 2.64v on the pot
00345 //   (values get smaller as the actuator retracts).  The pressure
00346 //   sensor is quite noisy.  A reasonable curve fit for pressure
00347 //   values above 1 volt as a function of pot voltage is:
00348 //
00349 //      pressure = 0.40480 pot^2 - 3.56259 pot + 8.69659
00350 //
00351 int devbrake::calibrate_brake(void)
00352 {
00353   int rc;
00354 
00355   rc = read_stable_value(&devbrake::query_pot, &pot_off, POT_EPSILON);
00356   if (rc != 0) return rc;
00357 
00358   rc = read_stable_value(&devbrake::query_pressure, &pressure_min,
00359                          PRESS_EPSILON);
00360   if (rc != 0) return rc;
00361 
00362   // Minimum brake pressure needs to be below 1 volt or both 
00363   // throttle and brake will be active simultaneously (very bad).
00364   if (pressure_min > 1.0) {
00365     ROS_ERROR("Minimum brake pressure too high: %.3f", pressure_min);
00366     return (1);
00367   }
00368 
00369   // apply full brake: pull cable for 8 sec, stop if limit reached
00370   for (int timeout = 8*10; timeout > 0; --timeout)
00371     {
00372       rc = encoder_goto(5000);          // relative goto
00373       if (rc != 0) return rc;
00374       if (cur_status & Status_Bp)       // +limit reached?
00375         break;
00376       usleep(50*1000);                  // wait 0.05 sec
00377     }
00378 
00379   // log whether or not brake reached +limit within 4 sec.
00380   if (cur_status & Status_Bp)   // +limit reached?
00381     ROS_INFO("Good: +limit reached during calibration");
00382   else
00383     ROS_WARN("Bad: +limit not reached within 4 sec. calibration");
00384 
00385   rc = read_stable_value(&devbrake::query_encoder, &encoder_max, ENC_EPSILON);
00386   if (rc != 0) return rc;
00387 
00388   rc = read_stable_value(&devbrake::query_pot, &pot_full, POT_EPSILON);
00389   if (rc != 0) return rc;
00390 
00391   rc = read_stable_value(&devbrake::query_pressure, &pressure_max,
00392                          PRESS_EPSILON);
00393   if (rc != 0) return rc;
00394 
00395   cur_position = 1.0;                   // consider this position fully on
00396   encoder_range = encoder_max - encoder_min;
00397   pot_range = pot_full - pot_off;       // this will be negative
00398   pressure_range = pressure_max - pressure_min;
00399 
00400   ROS_INFO("calibrated encoder range is [%.f, %.f]", encoder_min, encoder_max);
00401   ROS_INFO("calibrated potentiometer range is [%.3f, %.3f]", pot_off, pot_full);
00402   ROS_INFO("calibrated pressure range is [%.3f, %.3f]",
00403            pressure_min, pressure_max);
00404 
00405   return 0;
00406 }
00407 
00408 // Sometimes the motor slips, forcing us to adjust the encoder limits
00409 // used for braking requests.  Since we assume any out of range value
00410 // is due to slippage, always adjust both max and min whenever either
00411 // is exceeded, leaving the encoder_range the same.
00412 //
00413 // We could request the motor to set a new origin (O=val), but this
00414 // implementation just adjusts the limits used when calculating
00415 // position requests.
00416 inline void devbrake::check_encoder_limits(void)
00417 {
00418   // Make no adjustments before calibration is complete,
00419   // calibrate_brake() determines these limits.
00420   if (already_configured == false)
00421     return;
00422 
00423   if (cur_encoder > encoder_max)
00424     {
00425       encoder_min += cur_encoder - encoder_max;
00426       encoder_max = cur_encoder;
00427     }
00428   else if (cur_encoder < encoder_min)
00429     {
00430       encoder_max -= encoder_min - cur_encoder;
00431       encoder_min = cur_encoder;
00432     }
00433   else                                  // still inside normal range
00434     return;
00435 
00436   ROS_INFO("Brake motor slipped!  New encoder range is [%.f, %.f]",
00437            encoder_min, encoder_max);
00438 }
00439 
00440 // Configure the brake controller using position mode.
00441 //
00442 // Interesting facts:
00443 //   The brake actuator has a four-inch throw.
00444 //   There is about a two-inch range with the cable connected.
00445 //   The worm gear has a 3:1 ratio.
00446 //   The worm screw advances the actuator 1/4 inch per rev.
00447 //   The motor has 2000 position ticks per revolution.
00448 //   Therefore, the full range is about 96000 ticks.
00449 //   The 10-bit potentiometer on A/D port E has a 5-volt range.
00450 //   The velocity parameter units are rev/sec * 32,212
00451 //   The acceleration parameter units are rev/sec/sec * 7.91
00452 //
00453 // NOTE: the potentiometer is returning high values (near 1000) when
00454 // the brake is off, and low values (near 100) when fully applied.
00455 //
00456 // To remove brake spring use 3/16" allen wrench and 11mm open end wrench.
00457 //
00458 int devbrake::configure_brake(void)
00459 {
00460   int rc;
00461 
00462   rc = servo_cmd("ZS RW\n");            // reset all status bits
00463   if (rc != 0) return rc;
00464     
00465   rc = servo_cmd("MP D=0 G RW\n");      // ensure position mode
00466   if (rc != 0) return rc;
00467     
00468   rc = servo_cmd("UAI RW\n");           // set Port A as an input
00469   if (rc != 0) return rc;
00470     
00471   rc = servo_cmd("O=0 RW G\n");         // set temporary origin
00472   if (rc != 0) return rc;
00473     
00474   rc = servo_cmd("A=8*96 RW G\n");      // set acceleration: 8 in/sec/sec
00475   if (rc != 0) return rc;
00476     
00477   rc = servo_cmd("V=32212*48 RW G\n");  // set velocity: 4" in 1 sec
00478   if (rc != 0) return rc;
00479     
00480   rc = servo_cmd("LIMD RW\n");          // enable directional limits
00481   if (rc != 0) return rc;
00482     
00483   rc = servo_cmd("LIML RW\n");          // set limits active low
00484   if (rc != 0) return rc;
00485     
00486   rc = servo_cmd("UCP RW\n");           // set pin C to +limit
00487   if (rc != 0) return rc;
00488     
00489   rc = servo_cmd("UDM RW\n");           // set pin D to -limit
00490   if (rc != 0) return rc;
00491     
00492   rc = servo_cmd("F=1 RW\n");           // stop after limit fault
00493   if (rc != 0) return rc;
00494     
00495   // find negative limit position
00496   for (int timeout = 4*10; timeout > 0; --timeout)
00497     {
00498       rc = encoder_goto(-10000);        // relative goto
00499       if (rc != 0) return rc;
00500       if (cur_status & Status_Bm)       // -limit reached?
00501         {
00502           ROS_INFO("-limit reached during configuration");
00503           break;
00504         }
00505       usleep(100*1000);                 // wait 0.1 sec
00506     }
00507 
00508   if ((cur_status & Status_Bm) == 0)    // -limit not reached?
00509     {
00510       ROS_ERROR("Brake failure: "
00511                 "unable to reach negative limit in 4 seconds.");
00512       return ENODEV;
00513     }
00514 
00515   rc = servo_cmd("O=0 RW\n");           // set the origin here
00516   if (rc != 0) return rc;
00517 
00518   // initialize position values
00519   encoder_min = 0.0;
00520   encoder_range = encoder_max - encoder_min;
00521   cur_position = 0.0;
00522 
00523   return 0;
00524 }
00525 
00526 // send encoder position relative displacement command
00527 //
00528 // When the brake clutch slips, encoder values for a given position
00529 // change.  Therefore, we avoid using absolute encoder position
00530 // commands (P=?).  If a positive (full brake) or negative (brake off)
00531 // limit has been reached, recalibrate the encoder limits to agree
00532 // with the physical position switches.
00533 //
00534 // TODO: at some point, the encoder could overflow if it keeps
00535 // slipping.  Resetting the origin would prevent that.
00536 int devbrake::encoder_goto(int enc_delta)
00537 {
00538   sprintf(buffer, "D=%d RW G\n", enc_delta);
00539   int rc = servo_cmd(buffer);
00540   if (rc == 0)
00541     {
00542       if ((cur_status & Status_Bp) && (enc_delta >= 0)) // +limit reached?
00543         {
00544           float encoder_val;
00545           int qrc = query_encoder(&encoder_val);
00546           if (qrc != 0) return rc;
00547           if (already_configured)
00548             encoder_min += encoder_val - encoder_max;
00549           encoder_max = encoder_val;
00550           encoder_range = encoder_max - encoder_min;
00551           ROS_DEBUG("Brake +limit reached, status: 0x%04x, "
00552                     "encoder range [%.f, %.f]",
00553                     cur_status, encoder_min, encoder_max);
00554 
00555           // see if pot_full needs adjustment
00556           float pot_val;
00557           qrc = query_pot(&pot_val);
00558           if (qrc != 0) return rc;
00559           if (fabs(pot_val-pot_full) > POT_EPSILON)
00560             {
00561               pot_full = pot_val;
00562               pot_range = pot_full - pot_off; // this will be negative
00563               ROS_DEBUG("   new potentiometer range is [%.3f, %.3f]",
00564                         pot_off, pot_full);
00565             }
00566         }
00567       if ((cur_status & Status_Bm) && (enc_delta <= 0)) // -limit reached?
00568         {
00569           float encoder_val;
00570           int qrc = query_encoder(&encoder_val);
00571           if (qrc != 0) return rc;
00572           if (already_configured)
00573             encoder_max += encoder_val - encoder_min;
00574           encoder_min = encoder_val;
00575           encoder_range = encoder_max - encoder_min;
00576 
00577           // TODO: reset encoder origin if motor has slipped
00578           //if (encoder_min != 0.0) {
00579           //  servo_cmd("MP D=0 O=%d RW G\n", (int) rintf(encoder_min));
00580           //  encoder_max += encoder_min;
00581           //  encoder_min = 0.0;
00582           //}
00583 
00584           ROS_DEBUG("Brake -limit reached, status: 0x%04x, "
00585                     "encoder range [%.f, %.f]",
00586                     cur_status, encoder_min, encoder_max);
00587 
00588           // see if pot_off needs adjustment
00589           float pot_val;
00590           qrc = query_pot(&pot_val);
00591           if (qrc != 0) return rc;
00592           if (fabs(pot_val-pot_off) > POT_EPSILON)
00593             {
00594               pot_off = pot_val;
00595               pot_range = pot_full - pot_off; // this will be negative
00596               ROS_DEBUG("   new potentiometer range is [%.3f, %.3f]",
00597                         pot_off, pot_full);
00598             }
00599         }
00600     }
00601   return rc;
00602 }
00603 
00604 /*  Query status from the brake controller.
00605  *
00606  *   Writes a command, then reads up to nbytes of status from the device.
00607  *
00608  *  returns: 0 if successful, errno value otherwise.
00609  */
00610 int devbrake::query_cmd(const char *string, char *status, int nbytes)
00611 {
00612   ROS_DEBUG("query_cmd %s", string);
00613   int len = strlen(string);
00614   memset(status, 0, nbytes);
00615 
00616   // interpret actuator command and response, if simulated device
00617   if (sim)
00618     return sim->interpret(string, status, nbytes);
00619 
00620   int rc=0;
00621   int attempts = 3;                     // number of times to try command
00622   do
00623     {
00624       // Flush the I/O buffers to ensure nothing is left over from any
00625       // previous command.
00626       tcflush(fd, TCIOFLUSH);
00627 
00628       // There is not much point in checking for errors on the
00629       // write().  If something went wrong, we'll find out by reading
00630       // the device status.
00631       int res = write(fd, string, len);
00632       if (res < 0)
00633         ROS_ERROR_THROTTLE(100, "write() error: %d", errno);
00634 
00635       int timeout = 16;                 // set timeout in msecs
00636       int linelen = 0;
00637 
00638       int64_t start_time = GetTime();
00639       int64_t stop_time = start_time + timeout;
00640 
00641       while (true)
00642         {
00643           if (timeout >= 0) {
00644             struct pollfd fds[1];
00645             fds[0].fd=fd;
00646             fds[0].events = POLLIN;
00647             int64_t delay = stop_time - GetTime();
00648             if (delay < 0) delay = 0;
00649             int retval = ::poll(fds, 1, delay);
00650             if (retval < 0)
00651               {
00652                 if (errno == EINTR)
00653                   continue;
00654                 ROS_ERROR("error returned on poll");
00655                 rc= errno;
00656                 break;
00657               }
00658             else if (!retval)
00659               {
00660                 ROS_INFO("timeout on poll");
00661                 rc= EBUSY;
00662                 break;
00663               }
00664             else {
00665               if ((fds[0].revents & POLLERR) ||
00666                   (fds[0].revents & POLLHUP) ||
00667                   (fds[0].revents & POLLNVAL))
00668                 {
00669                   ROS_ERROR("Device error on poll");
00670                   rc= EIO;
00671                   break;
00672                 }
00673             }
00674           }
00675 
00676           int bytes = read(fd, status + linelen, 1);
00677           if (bytes < 0)
00678             {
00679               if (errno == EINTR)
00680                 continue;
00681               ROS_ERROR("error: %s", strerror(rc));
00682               rc = errno;
00683               break;                    // retry servo command
00684             }
00685 
00686           ROS_DEBUG("read() returns %d, 0x%02X (%c)",
00687                     bytes, (bytes > 0? status[linelen]: 0),
00688                     (bytes > 0? status[linelen]: '-'));
00689           
00690           if (bytes==0)
00691             continue;
00692           else if (status[linelen] != '\r') // not end of line?
00693             {
00694               // have a new character
00695               if (++linelen >= nbytes)
00696                 {
00697                   ROS_ERROR("buffer overflow: %s", status);
00698                   rc = ENOSPC;
00699                   break;                // retry servo command
00700                 }
00701             }
00702           else                          // have a complete line
00703             {
00704               return 0;                 // success
00705             }
00706         }
00707     }
00708   while (--attempts > 0);               // retry, if error
00709       
00710   return rc;
00711 }
00712 
00713 // read a stable brake sensor value SLOWLY -- use only during setup()
00714 //
00715 //  Note that the potentiometer returns smaller values when the brake
00716 //  is full engaged; the actuator retracts while pulling on the cable.
00717 //
00718 //  Beware that these sensor readings will often increase or decrease
00719 //  after reaching a minimum due to slippage or hitting the limit
00720 //  switch.  The readings will not always change monotonically.
00721 //
00722 //  returns: 0 if successful, with value updated;
00723 //           errno value otherwise, value unchanged
00724 int devbrake::read_stable_value(query_method_t query_method,
00725                                 double *value, float epsilon)
00726 {
00727   float prev_filter;
00728   float prev_val;
00729   int rc = (this->*query_method)(&prev_val);
00730   if (rc != 0) goto ioerror;
00731   prev_filter = prev_val;               // initial filter state
00732 
00733   // loop until value settles
00734   for (int timeout = 4*10; timeout > 0; --timeout)
00735     {
00736       usleep(100*1000);                 // wait 0.1 sec
00737 
00738       float cur_val;
00739       rc = (this->*query_method)(&cur_val);
00740       if (rc != 0) goto ioerror;
00741 
00742       // Experimental: smooth the data using an RC low-pass filter to
00743       // eliminate small fluctuations.
00744       float cur_filter = RC_filter(0.5, cur_val, prev_filter);
00745       ROS_DEBUG("RC filter output = %.3f, previous = %.3f",
00746                 cur_filter, prev_filter);
00747 
00748       // Once in a while, the sensor may return a bogus value.  Only
00749       // consider cur_val valid when it is within epsilon of the
00750       // previous reading, indicating the brake has settled to a
00751       // stable, achievable reading.
00752 #undef USE_FILTER
00753 #ifdef USE_FILTER
00754       if (fabs(cur_filter - prev_filter) <= epsilon)
00755 #else
00756       if (fabs(cur_val - prev_val) <= epsilon)
00757 #endif
00758         {
00759           *value = cur_val;
00760           return 0;                     // success
00761         }
00762       prev_val = cur_val;
00763       prev_filter = cur_filter;
00764     }
00765 
00766   ROS_ERROR("brake calibration failed! (4 sec. timeout)");
00767   return EBUSY;
00768 
00769  ioerror:
00770   ROS_ERROR("brake calibration failed! (%s)", strerror(rc));
00771   return rc;
00772 }
00773 
00774 /* Write a command to the brake controller, log status response.
00775  *
00776  *  The command *must* include a status request (RS or RW).
00777  *
00778  *  returns: 0 if successful, updating cur_status;
00779  *           errno value otherwise.
00780  */
00781 int devbrake::servo_cmd(const char *string)
00782 {
00783   char response[MAX_SERVO_CMD_BUFFER];
00784   int rc;
00785 
00786   // use query_cmd() to read the status response correctly
00787   rc = query_cmd(string, response, MAX_SERVO_CMD_BUFFER);
00788   if (rc == 0)
00789     {
00790       cur_status = (brake_status_t) atoi(response);
00791       // check for really bad status like Ba, Be, Bh
00792       if (cur_status & (Status_Ba|Status_Be|Status_Bh))
00793         ROS_WARN("bad brake status = %d (0x%02x)", cur_status, cur_status);
00794       else
00795         ROS_DEBUG("brake status = %d (0x%02x)", cur_status, cur_status);
00796     }
00797 
00798   return rc;
00799 }
00800 
00801 /* Write a command to the brake controller, no response expected.
00802  *
00803  *  The command *must* *not* include a status request (RS or RW).
00804  *
00805  *  returns: no indication of whether it worked.
00806  */
00807 void devbrake::servo_write_only(const char *string)
00808 {
00809   ROS_DEBUG("servo_write_only %s", string);
00810 
00811   // Flush the I/O buffers to ensure nothing is left over from any
00812   // previous command.
00813   tcflush(fd, TCIOFLUSH);
00814 
00815   // There is not much point in checking for errors on the write().
00816   // If something went wrong, we'll find out later on some command
00817   // that reads status.
00818   int res = write(fd, string, strlen(string));
00819   if (res < 0)
00820     ROS_ERROR_THROTTLE(100, "write() error: %d", errno);
00821 }


art_servo
Author(s): Austin Robot Technology, Jack O'Quin
autogenerated on Fri Jan 3 2014 11:09:12