model_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: model_brake.cc 2 2010-01-17 01:54:03Z jack.oquin $
00007  */
00008 
00017 #include <errno.h>
00018 #include <stdio.h>
00019 
00020 #include <art/conversions.h>
00021 #include "model_brake.h"
00022 
00023 namespace
00024 {
00025   static double EPSILON_SECONDS = 0.000001;
00026   static double EPSILON_TICKS = 0.5;
00027 }
00028 
00030 ArtBrakeModel::ArtBrakeModel(float init_pos)
00031 {
00032   am_ = new Animatics;
00033 
00034   // use private node handle to get parameters
00035   ros::NodeHandle mynh("~");
00036 
00037   // configure actuator velocity and acceleration
00038   double accel_limit_in, max_vel_in;
00039   mynh.param("actuator_accel", accel_limit_in, 8.0);
00040   ROS_INFO("model actuator acceleration as %.2f in/s^2", accel_limit_in);
00041   mynh.param("actuator_max_vel", max_vel_in, 4.0);
00042   ROS_INFO("model max actuator velocity as %.2f in/s", max_vel_in);
00043 
00044   // convert configured values from inches to ticks
00045   double ticks_per_inch = 24000;
00046   actuator_accel_ = rint(accel_limit_in * ticks_per_inch);
00047   actuator_max_vel_ = rint(max_vel_in * ticks_per_inch);
00048 
00049   // initialize simulation data
00050   sim_status_ = 0;
00051   brake_position_ = init_pos;
00052   sim_encoder_ = encoder_goal_ = am_->pos2enc(brake_position_);
00053   x_ = (double) sim_encoder_;
00054   a_ = 0;
00055   v_ = 0;
00056   update_sensors(brake_position_);
00057   last_update_time_ = ros::Time::now().toSec();
00058 }
00059 
00060 ArtBrakeModel::~ArtBrakeModel()
00061 {
00062   delete am_;
00063 }
00064 
00070 int ArtBrakeModel::interpret(const char *string, char *status, int nbytes)
00071 {
00072   // preset empty status response
00073   if (nbytes > 0)
00074     status[0] = '\0';
00075 
00076   // avoid problems with empty command
00077   if (string == NULL)
00078     return EIO;
00079 
00080   // Bring the simulation up to date
00081   update();
00082 
00083   int retval = 0;                       // return value
00084 
00085   // Much simpler than a real parser: branch on first character.
00086   switch (string[0])
00087     {
00088     case 'c':
00089       if (0 == strcmp(string, "c=UEA Rc\n"))
00090         {
00091           // read potentiometer A/D value
00092           snprintf(status, nbytes, "%d\n", digital_potentiometer_);
00093         }
00094       else if (0 == strcmp(string, "c=UAA Rc\n"))
00095         {
00096           // read pressure A/D values
00097           snprintf(status, nbytes, "%d\n", digital_pressure_);
00098         }
00099       break;
00100 
00101     case 'D':
00102       // start relative actuator motion, read status word
00103       int encoder_delta;
00104       if (1 == sscanf(string, "D=%d RW G\n", &encoder_delta))
00105         {
00106           // TODO: force update() before changing goal or accel
00107           encoder_goal_ = am_->clamp_encoder(sim_encoder_ + encoder_delta);
00108 
00109           // Plan actuator relative move.
00110           plan(encoder_delta);
00111 
00112           ROS_DEBUG("simulated encoder delta: %d, goal: %d",
00113                     encoder_delta, encoder_goal_);
00114           sim_status_ |= Status_Bt;     // set trajectory busy
00115           snprintf(status, nbytes, "%d\n", sim_status_);
00116         }
00117       else
00118         retval = EIO;                   // parse failure
00119       break;
00120 
00121     case 'R':
00122       if (0 == strcmp(string, "RP\n"))
00123         {
00124           // read encoder position value
00125           snprintf(status, nbytes, "%d\n", sim_encoder_);
00126         }
00127       break;
00128 
00129     case 'Z':
00130       if (0 == strcmp(string, "ZS RW\n"))
00131         {
00132           // reset status word
00133           sim_status_ = 0;
00134           snprintf(status, nbytes, "%d\n", sim_status_);
00135         }
00136       break;
00137 
00138     default:
00139       // return "success" with empty status
00140       ;
00141     }
00142 
00143   return retval;
00144 }
00145 
00154 void ArtBrakeModel::move(double start, double finish)
00155 {
00156   double t = start;
00157   while (t < finish)
00158     {
00159       a_ = plan_.next(t);               // get next acceleration
00160       double dt = plan_.interval(t, finish);
00161       double v0 = v_;                   // previous velocity
00162       v_ += dt * a_;                    // velocity change
00163       x_ += dt * (v_ + v0)/2.0;         // advance by average velocity
00164       t += dt;                          // advance simulated time
00165 #if 0
00166       // Warn if encoder output outside configured range (not really
00167       // an error, it can happen with the real device).
00168       if ((x_ < am_->encoder_min_ - EPSILON_TICKS)
00169           || (x_ > am_->encoder_max_ + EPSILON_TICKS))
00170         ROS_WARN("actuator out of range: (time %.6f): %.f, %.f, %.f",
00171                  t, x_, v_, a_);
00172 #endif
00173       // Clamp encoder output to configured range (probably more
00174       // realistic not to do this).
00175       if (x_ < am_->encoder_min_ - EPSILON_TICKS)
00176         x_ = am_->encoder_min_;
00177       else if (x_ > am_->encoder_max_ + EPSILON_TICKS)
00178         x_ = am_->encoder_max_;
00179       ROS_DEBUG("actuator (time %.6f): %.f, %.f, %.f (%.6f)",
00180                 t, x_, v_, a_, dt);
00181     }
00182 
00183   if (fabs(finish-t) >= EPSILON_SECONDS)
00184     ROS_WARN("ArtBrakeModel::move() finished early (%.6f < %.6f)", t, finish);
00185 }
00186 
00206 double ArtBrakeModel::plan(double dx)
00207 {
00208   double t0 = ros::Time::now().toSec();
00209   double a;                             // required actuator acceleration
00210 
00211   if (fabs(dx) > EPSILON_TICKS)         // nonzero move?
00212     a = getAccel(dx);                   // accelerate in new direction
00213   else if (fabs(v_) > EPSILON_TICKS)    // current velocity nonzero?
00214     a = getAccel(-v_);                  // cancel current velocity
00215   else                                  // holding in place
00216     {
00217       plan_.reset();                    // delete any existing plan
00218       return 0.0;
00219     }
00220 
00221   double dt_h = -v_/a;                  // time to halt current movement
00222   double dx_h = dx - 0.5 * v_ * dt_h;   // dx remaining after halt
00223   double dt2 = sqrt(dx_h/a);            // deceleration time
00224   double dt1 = dt2 + dt_h;              // time moving towards goal
00225   double dt = dt1 + dt2;                // total move duration
00226 
00227   // make a new plan
00228   plan_.reset();                        // delete current plan
00229   if (dt1 > EPSILON_SECONDS)
00230     plan_.append(t0 + dt1, a);
00231   if (dt2 > EPSILON_SECONDS)
00232     plan_.append(t0 + dt, -a);
00233 
00234   // log some debug information
00235   plan_.log();
00236   ROS_DEBUG("plan(%.f) time %.6f, dt: %.6f, dt1: %.6f, dt2: %.6f",
00237             dx, t0, dt, dt1, dt2);
00238 
00239   return dt;
00240 }
00241 
00243 void ArtBrakeModel::update(void)
00244 {
00245   double now = ros::Time::now().toSec();
00246 
00247   // model actuator movement since last update
00248 #if 1
00249   move(last_update_time_, now);
00250   sim_encoder_ = rint(x_);
00251 #else
00252   sim_encoder_ = encoder_goal_;         // arm movement finished
00253 #endif
00254   if (plan_.finished())                 // finished moving?
00255     sim_status_ &= (~Status_Bt);        // reset trajectory busy
00256 
00257   // compute fractional position from encoder value
00258   brake_position_ = limit_travel(am_->enc2pos(sim_encoder_));
00259 
00260   if (sim_encoder_ <= am_->encoder_min_)
00261     {
00262       sim_status_ |= (Status_Bm | Status_Bl);
00263       ROS_DEBUG("-limit reached, status: 0x%.02x", sim_status_);
00264     }
00265   else
00266     sim_status_ &= (~Status_Bm);
00267 
00268   if (sim_encoder_ >= am_->encoder_max_)
00269     {
00270       sim_status_ |= (Status_Bp | Status_Br);
00271       ROS_DEBUG("+limit reached, status: 0x%.02x", sim_status_);
00272     }
00273   else
00274     sim_status_ &= (~Status_Bp);
00275 
00276   update_sensors(brake_position_);
00277 
00278   ROS_DEBUG("Brake model: 0x%02x [%.3f, %d, %d, %d] %.6f",
00279             sim_status_, brake_position_, digital_potentiometer_,
00280             digital_pressure_, sim_encoder_, now);
00281 
00282   last_update_time_ = now;
00283 }
00284 
00285 void ArtBrakeModel::update_sensors(float position)
00286 {
00287   digital_pressure_ =
00288     analog_to_digital(am_->pos2press(position), 5.0, 10); 
00289   digital_potentiometer_ =
00290     analog_to_digital(am_->pos2pot(position), 5.0, 10);
00291 }
 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