devsteer.cc
Go to the documentation of this file.
00001 /*
00002  *  ART steering servo controller device interface
00003  *
00004  *  Copyright (C) 2008 Austin Robot Technology
00005  *  License: Modified BSD Software License Agreement
00006  *
00007  *  $Id: devsteer.cc 1927 2012-01-09 21:58:06Z austinrobot $
00008  */
00009 
00010 #include <fcntl.h>
00011 #include <termios.h>
00012 #include <poll.h>
00013 
00014 #include <ros/ros.h>
00015 #include <art_msgs/ArtHertz.h>
00016 #include <art_msgs/ArtVehicle.h>
00017 #include "devsteer.h"
00018 #include "silverlode.h"
00019 
00021 // public methods
00023 
00024 
00025 #define CLASS "devsteer"
00026 
00027 // Get the time (in ms)
00028 int64_t devsteer::GetTime()
00029 {
00030   ros::Time t = ros::Time::now();
00031   return (int64_t) t.sec * 1000 + (int64_t) t.nsec / 1000000;
00032 }
00033 
00034 devsteer::devsteer(int32_t center):
00035   req_angle_(0.0),
00036   center_ticks_(center)                 // for unit testing
00037 {}
00038 
00039 int devsteer::Open()
00040 {
00041   // open the serial port
00042   int rc = this->Servo::Open(port_.c_str(), (O_RDWR|O_NOCTTY|O_NONBLOCK));
00043   if (fd < 0) {
00044     ROS_ERROR("Couldn't open %s (%s)", port_.c_str(), strerror(errno));
00045     return -1;
00046   }
00047     
00048   // set actual baud rate
00049   rc = configure_raw_port((B57600|CS8), 0);
00050   if (rc != 0) goto fail;
00051 
00052   rc = configure_steering();
00053   if (rc != 0) goto fail;
00054 
00055   req_angle_ = starting_angle_ = 0.0;   // initialize position
00056   starting_ticks_ = 0;                  //   assuming wheel is centered
00057 
00058   return rc;
00059 
00060  fail:
00061   this->Servo::Close();
00062   return rc;                            // Open() failed
00063 }
00064 
00065 int devsteer::Close()
00066 {
00067   if (center_on_exit_)
00068     steering_absolute(0.0);             // center steering wheel
00069   return this->Servo::Close();
00070 }
00071 
00072 int devsteer::Configure()
00073 {
00074   // use private node handle to get parameters
00075   ros::NodeHandle private_nh("~");
00076 
00077   private_nh.param("port", port_, std::string("/dev/steering"));
00078   ROS_INFO_STREAM("steering port = " << port_);
00079 
00080   private_nh.param("center_on_exit", center_on_exit_, false);
00081   if (center_on_exit_)
00082     ROS_INFO("center steering on exit");
00083   else
00084     ROS_INFO("do not center steering on exit");
00085 
00086   private_nh.param("training", training_, false);
00087   if (training_)
00088     ROS_INFO("using training mode");
00089 
00090   private_nh.param("simulate_moving_errors",
00091                    simulate_moving_errors_, false);
00092   if (simulate_moving_errors_)
00093     ROS_INFO("simulating intermittent moving errors");
00094 
00095   if (training_)
00096     ROS_INFO("using training mode");
00097 
00098   steering_rate_ = art_msgs::ArtVehicle::max_steer_degrees / 2.0;
00099   private_nh.getParam("steering_rate", steering_rate_);
00100   ROS_INFO("steering rate is %.2f degrees/sec.", steering_rate_);
00101 
00102   return 0;
00103 }
00104 
00110 int devsteer::check_status(void)
00111 {
00112   int rc = get_status_word(diag_msg_.status_word);
00113 
00114   if (rc == 0)
00115     {
00116       // internal status register bits to check
00117       uint16_t bad_status = (silverlode::isw::moving_error);
00118       uint16_t required_status = (silverlode::isw::temp_driver_en);
00119       if ((diag_msg_.status_word & bad_status) != 0
00120           || (diag_msg_.status_word & required_status) != required_status)
00121         {
00122           ROS_ERROR("SilverLode internal status error: 0x%04x",
00123                    diag_msg_.status_word);
00124           rc = EIO;
00125         }
00126     }
00127 
00128   return rc;
00129 }
00130 
00144 int devsteer::get_angle(float &degrees)
00145 {
00146   int rc = 0;
00147 
00148   if (have_tty)                         // using actual device?
00149     {
00150       int32_t iticks;
00151       rc = get_encoder(iticks);
00152       if (rc == 0)
00153         {
00154           degrees = ticks2degrees(iticks);
00155         }
00156       else
00157         {
00158           ROS_WARN("encoder read failure, cannot estimate position");
00159         }
00160     }
00161   else
00162     {
00163       // simulate steering motion as a constant angular velocity
00164       float remaining_angle = req_angle_ - degrees;
00165       float degrees_per_cycle = (steering_rate_ /
00166                                  art_msgs::ArtHertz::STEERING);
00167 
00168       DBG("remaining angle %.3f, degrees per cycle %.3f",
00169           remaining_angle, degrees_per_cycle);
00170 
00171       if (fabs(remaining_angle) <= degrees_per_cycle)
00172         {
00173           degrees = req_angle_;
00174         }
00175       else
00176         {
00177           degrees += ((remaining_angle >= 0.0)?
00178                       degrees_per_cycle: -degrees_per_cycle);
00179         }
00180       ROS_DEBUG("simulated angle = %.2f degrees", degrees);
00181 
00182       // set corresponding (simulated) encoder value
00183       diag_msg_.encoder = degrees2ticks(degrees);
00184     }
00185 
00186   return rc;
00187 }
00188 
00197 int devsteer::get_encoder(int32_t &iticks)
00198 {
00199   int rc = send_cmd("@16 12 1\r");
00200   if (rc == 0 && have_tty)
00201     {
00202       // stage unit test: initialize buffer to encoder 329379.0 response
00203       //strncpy(buffer, "# 10 000C 0005 06A3", MAX_SERVO_CMD_BUFFER);
00204       unsigned int pos_high, pos_low;
00205       if (2 == sscanf(buffer, "# 10 000C %4x %4x", &pos_high, &pos_low))
00206         {
00207           iticks = (pos_high << 16) + pos_low;
00208           ROS_DEBUG(" " DEVICE " response: `%s'", buffer);
00209           diag_msg_.encoder = iticks;
00210         }
00211       else
00212         {
00213           ROS_INFO(" " DEVICE " unexpected response: `%s'", buffer);
00214           rc = EINVAL;
00215         }
00216     }
00217   return rc;
00218 }
00219 
00220 // get steering status word
00221 //
00222 // returns: 0 if successful, errno value otherwise
00223 //
00224 // isw = current status, if I/O successful
00225 //
00226 int devsteer::get_status_word(uint16_t &status)
00227 {
00228   int rc = send_cmd("@16 20\r");
00229   if (rc == 0)
00230     {
00231       if (have_tty)
00232         {
00233           unsigned int isw;
00234           if (1 == sscanf(buffer, "# 10 0014 %4x", &isw))
00235             {
00236               status = isw;
00237               ROS_DEBUG(" " DEVICE " status: `%s'", buffer);
00238             }
00239           else
00240             {
00241               ROS_WARN(" " DEVICE " unexpected response: `%s'", buffer);
00242               rc = EINVAL;
00243             }
00244         }
00245       else
00246         {
00247           status = silverlode::isw::temp_driver_en;
00248         }
00249 
00250       if (simulate_moving_errors_)
00251         {
00252           // Hack: set moving error for four of every 64 seconds
00253           ros::Time now = ros::Time::now();
00254           if ((now.sec & 0x003C) == 0)
00255             status |= silverlode::isw::moving_error;
00256         }
00257     }
00258   return rc;
00259 }
00260 
00266 void devsteer::publish_diag(const ros::Publisher &diag_pub)
00267 {
00268   get_encoder(diag_msg_.encoder);
00269   diag_msg_.header.stamp = ros::Time::now();
00270   diag_pub.publish(diag_msg_);
00271 }
00272 
00284 int devsteer::set_initial_angle(float position)
00285 {
00286   int rc = 0;
00287 
00288 #if 0
00289 
00290   // Set starting_ticks to current encoder position.  This will be
00291   // (approximately) zero after initial power-on of the controller,
00292   // but may vary in subsequent runs if the controller has not been
00293   // reset.  That variable is used by degrees2ticks() as a conversion
00294   // offset.
00295 
00296   // BUG: should not rely on get_encoder() before setting
00297   // center_ticks_ when using real device with simulated sensor.
00298   rc = get_encoder(starting_ticks_);
00299   if (rc == 0)
00300     {
00301       // Since starting_angle is the current wheel angle, its negative
00302       // is the offset of the center wheel position.
00303       starting_angle_ = position;
00304       center_ticks_ = 
00305         (int32_t) lrint(-starting_angle_ * TICKS_PER_DEGREE) + starting_ticks_;
00306       diag_msg_.center_ticks = center_ticks_;
00307 
00308       ROS_INFO("starting ticks = %d, center ticks = %d",
00309                starting_ticks_, center_ticks_);
00310 
00311       // Attempt to set encoder soft stop limits.  If that fails, run
00312       // without them.  They are a safety net, limiting travel to 30
00313       // degrees from the center position.  This driver should never
00314       // request a position more than 29 degrees from center.  The
00315       // mechanical steering limit is about 31 degrees.  We avoid
00316       // hitting that limit lest it damage the stepper motor or its
00317       // linkage.
00318       write_register(39, center_ticks_ - 300001); // soft stop lower
00319       write_register(40, center_ticks_ + 300001); // soft stop upper
00320 
00321       // Send Profile Move Continuous (PMC) command.  If successful,
00322       // from now on any value written to data register 20 will
00323       // immediately cause the wheel to seek that position.  If it
00324       // fails, the device is inoperable and the driver shuts down.
00325       rc = servo_cmd("@16 240 0 0\r");
00326       if (rc != 0)
00327         ROS_WARN(DEVICE " failed to enter PMC mode.");
00328 
00329       // TODO check if KMC and KMR should be moved here
00330     }
00331   else
00332     ROS_WARN(DEVICE " failed reading steering encoder: "
00333              "initial position may be wrong!");
00334 
00335 #else
00336 
00337   // Since starting_angle is our current wheel angle, write the
00338   // corresponding encoder position to register 20.  Since the PMC
00339   // command has not been issued, this does not cause any movement.
00340   // Subsequently, encoder position zero will correspond to wheel
00341   // angle zero.
00342   starting_angle_ = position;
00343   center_ticks_ = 0;                    // TODO: remove obsolete variable?
00344   diag_msg_.center_ticks = center_ticks_;
00345   starting_ticks_ = degrees2ticks(starting_angle_);
00346   ROS_INFO("starting ticks = %d, center ticks = %d",
00347            starting_ticks_, center_ticks_);
00348   rc = write_register(20, starting_ticks_); // initial wheel position
00349   if (rc != 0)
00350     {
00351       ROS_ERROR(DEVICE " failed to set initial wheel encoder position.");
00352       return rc;                        // device failure
00353     }
00354 
00355   // Attempt to set encoder soft stop limits, but if it fails run
00356   // without them.  They are a safety net, limiting travel to 30
00357   // degrees from the center position.  This driver should never
00358   // request a position more than 29 degrees from center.  The
00359   // mechanical steering limit is about 31 degrees.  We avoid hitting
00360   // that limit lest it damage the stepper motor or its linkage.
00361   write_register(39, center_ticks_ - 300001); // soft stop lower
00362   write_register(40, center_ticks_ + 300001); // soft stop upper
00363 
00364   // Send Profile Move Continuous (PMC) command.  If successful, from
00365   // now on any value written to data register 20 will immediately
00366   // cause the wheel to seek that position.  If this command fails,
00367   // the device is inoperable and the driver will shut down.
00368   rc = servo_cmd("@16 240 0 0\r");
00369   if (rc != 0)
00370     ROS_ERROR(DEVICE " failed to enter PMC mode.");
00371 
00372 #endif  
00373 
00374   return rc;
00375 }
00376 
00377 int devsteer::steering_absolute(float position)
00378 {
00379   DBG("steering_absolute(%.3f)", position);
00380   req_angle_ = limit_travel(position);
00381   return encoder_goto(req_angle_);
00382 }
00383  
00384 int devsteer::steering_relative(float delta)
00385 {
00386   DBG("steering_relative(%.3f)", delta);
00387   return steering_absolute(req_angle_ + delta);
00388 }
00389 
00391 // private methods
00393 
00394 // Configure the steering controller for Profile Move Operation
00395 //
00396 int devsteer::configure_steering(void)
00397 {
00398   int rc;
00399 
00400 #if 0 // does not seem good (before or after RST)...
00401   rc = servo_cmd("@16 2\r");            // HLT: shut down all motion
00402   if (rc != 0) return rc;
00403 #endif
00404 
00405 #if 0 // should clear persistent KMC status, but does not work
00406   rc = servo_cmd("@16 163\r");          // CIS: clear internal status
00407   if (rc != 0) return rc;
00408   rc = servo_cmd("@16\r");              // POL: read polling status
00409   if (rc != 0) return rc;
00410   rc = servo_cmd("@16 1 65535\r");      // CPL: clear polling status
00411   if (rc != 0) return rc;
00412   rc = servo_cmd("@16 146\r");          // TTP: target to position
00413   if (rc != 0) return rc;
00414 
00415   // set KMC (Kill Motor Conditions) to stop motor for moving error
00416   rc = servo_cmd("@16 167 256 256\r"); 
00417   if (rc != 0) return rc;
00418 
00419   rc = servo_cmd("@16 227\r");          // enable motor drivers (EMD)
00420   if (rc != 0) return rc;
00421 #endif
00422 
00423 #if 1
00424   // The restart command branches to microcode address zero.
00425   // It never responds, and nothing works for a while afterward.
00426   servo_write_only("@16 4\r");          // RST: restart
00427   usleep(1000000);                      // wait for that to finish
00428 #endif
00429 
00430   // If the controller is in Profile Move Continuous (PMC) mode,
00431   // setting data registers 20 through 24 will have immediate and
00432   // undesired effects.  To avoid that, issue a PMX command first.
00433   //
00434   // If the controller was not in PMC mode, the PMX will fail, so
00435   // ignore any NAK response from that command.  (Should no longer be
00436   // a problem because of the RST.)
00437   // TODO remove this...
00438   servo_cmd("@16 242\r");               // PMX: exit profile move mode
00439 
00440   rc = write_register(20, 0);           // Reg 20 (Position) = 0
00441   if (rc != 0) return rc;
00442 
00443   // Max Acceleration 3865 = 1 RPS/s
00444   rc = write_register(21, 193274);     // Reg 21 (Accelera) = 25 RPS/s
00445   if (rc != 0) return rc;
00446 
00447   // Max Velocity (2^31/16,000) * (60RPM/RPS) = 8,053,064 = 1 RPS
00448   rc = write_register(22, 322122547);   // Reg 22 (Velocity) = 1 RPS
00449   if (rc != 0) return rc;
00450 
00451   rc = write_register(23, 193274);      // Reg 23 (Declerat) = -25 RPS/s
00452   if (rc != 0) return rc;
00453 
00454   rc = write_register(24, 0);           // Reg 24 (Offset) = 0
00455   if (rc != 0) return rc;
00456 
00457 #if 1
00458   // set KMC (Kill Motor Conditions) to stop motor for moving error
00459   servo_cmd("@16 167 256 256\r"); 
00460   //rc = servo_cmd("@16 167 0 0\r"); 
00461 
00462   // set KMR (Kill Motor Recovery) to "do nothing"
00463   servo_cmd("@16 181 0\r"); 
00464 #endif
00465 
00466   return rc;
00467 }
00468 
00469 // send encoder position absolute steering command
00470 int devsteer::encoder_goto(float degrees)
00471 {
00472   int32_t ticks = degrees2ticks(degrees);
00473   diag_msg_.last_request = ticks;
00474   ROS_DEBUG("setting steering angle to %.3f (%d ticks)", degrees, ticks);
00475 
00476   // Send Position to Stepper (Register 20)
00477   return write_register(20, ticks);
00478 }
00479 
00480 // Write a 32-bit integer value to a Quicksilver register.
00481 int devsteer::write_register(int reg, int32_t val)
00482 {
00483   char string[MAX_SERVO_CMD_BUFFER];
00484   ROS_DEBUG("writing %d to register %d", val, reg);
00485   snprintf(string, MAX_SERVO_CMD_BUFFER, "@16 11 %d %d\r", reg, val);
00486   return servo_cmd(string);
00487 }
00488 
00489 /*  Write a command to the Quicksilver.  The device returns a status
00490  *  string after receiving it.  Skip I/O if /dev/null.
00491  *
00492  *  returns: 0 if successful, errno value otherwise.
00493  */
00494 int devsteer::send_cmd(const char *string)
00495 {
00496   int len = strlen(string);
00497 
00498   // TODO: trailing \r in string messes up this message...
00499   ROS_DEBUG(" " DEVICE " command: `%s'", string);
00500 
00501   if (!have_tty)                        // null device will not respond
00502     {
00503       strncpy(buffer, "* 10", 4);       // fake acknowledgement response
00504       return 0;
00505     }
00506 
00507   int rc=0;
00508   int attempts = 3;                     // number of times to try command
00509   do
00510     {
00511       // There is not much point in checking for errors on the
00512       // write().  If something went wrong, we'll find out by reading
00513       // the device status.
00514       int res = write(fd, string, len);
00515       if (res < 0)
00516         ROS_ERROR_THROTTLE(100, "write() error: %d", errno);
00517 
00518       // Set timeout in msecs.  The device normally responds in two.
00519       int timeout = 20;
00520       int linelen = 0;
00521 
00522       int64_t start_time = GetTime();
00523       int64_t stop_time = start_time + timeout;
00524 
00525       while (true)
00526         {
00527           if (timeout >= 0) {
00528             struct pollfd fds[1];
00529             fds[0].fd=fd;
00530             fds[0].events = POLLIN;
00531             int64_t delay = stop_time - GetTime();
00532             if (delay < 0) delay = 0;
00533             int retval = ::poll(fds, 1, delay);
00534             if (retval < 0)
00535               {
00536                 if (errno == EINTR)
00537                   continue;
00538                 ROS_ERROR("error returned on poll");
00539                 rc= errno;
00540                 break;
00541               }
00542             else if (!retval)
00543               {
00544                 ROS_WARN("timeout on poll");
00545                 rc= EBUSY;
00546                 break;
00547               }
00548             else {
00549               if ((fds[0].revents & POLLERR) ||
00550                   (fds[0].revents & POLLHUP) ||
00551                   (fds[0].revents & POLLNVAL))
00552                 {
00553                   ROS_ERROR("Device error on poll");
00554                   rc= EIO;
00555                   break;
00556                 }
00557             }
00558           }
00559 
00560           int bytes = read(fd, buffer + linelen, 1);
00561           if (bytes < 0)
00562             {
00563               if (errno == EINTR)
00564                 continue;
00565               rc = errno;
00566               ROS_ERROR("error: %s", strerror(rc));
00567               break;                    // retry servo command
00568             }
00569           ROS_DEBUG_NAMED("details",
00570                           DEVICE " read() returns %d %c(0x%02X)",
00571                           bytes, buffer[linelen], buffer[linelen]);
00572           if (bytes==0)
00573             continue;
00574           else if (buffer[linelen] != '\r') // not end of line?
00575             {
00576               // have a new character
00577               if (++linelen >= MAX_SERVO_CMD_BUFFER)
00578                 {
00579                   ROS_ERROR(DEVICE " buffer overflow: %s", buffer);
00580                   rc = ENOSPC;
00581                   break;                // retry servo command
00582                 }
00583             }
00584           else                          // have a complete line
00585             {
00586               buffer[linelen] = '\0';   // EOL is end of string
00587               return 0;                 // success
00588             }
00589         }
00590 
00591       // operation failed, flush the I/O buffers
00592       tcflush(fd, TCIOFLUSH);
00593     }
00594   while (--attempts > 0);               // retry, if error
00595   
00596   return rc;
00597 }
00598 
00599 /*  Write a command to the Quicksilver.  Check that the status string
00600  *  returned is an acknowledgement.  Do nothing when in training mode.
00601  */
00602 int devsteer::servo_cmd(const char *string)
00603 {
00604   if (training_)
00605     return 0;                           // send no commands
00606 
00607   int rc = send_cmd(string);
00608   if ((rc == 0)                           // I/O successful?
00609       && strncmp(buffer, "* 10", 4) != 0) // no acknowledgement?
00610     {
00611       ROS_INFO(DEVICE " returned error: %s", buffer);
00612       rc = EIO;
00613     }
00614   return rc;
00615 }
00616 
00621 void devsteer::servo_write_only(const char *string)
00622 {
00623   ROS_DEBUG("servo_write_only %s", string);
00624 
00625   // Flush the I/O buffers to ensure nothing is left over from any
00626   // previous command.
00627   tcflush(fd, TCIOFLUSH);
00628 
00629   // There is not much point in checking for errors on the write().
00630   // If something went wrong, we'll find out later on some command
00631   // that reads status.
00632   int res = write(fd, string, strlen(string));
00633   if (res < 0)
00634     ROS_ERROR_THROTTLE(100, "write() error: %d", errno);
00635 }


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