accel_plan.cc
Go to the documentation of this file.
00001 /*
00002  *  Copyright (C) 2011 Austin Robot Technology
00003  *  License: Modified BSD Software License Agreement
00004  * 
00005  *  $Id: accel_plan.cc 1541 2011-05-09 19:07:23Z jack.oquin $
00006  */
00007 
00021 #include <ros/ros.h>
00022 #include <art/pid2.h>
00023 #include <art_msgs/ArtHertz.h>
00024 #include <art_msgs/Epsilon.h>
00025 
00026 #include "accel_plan.h"
00027 
00028 using art_msgs::Epsilon;
00029 
00030 namespace pilot
00031 {
00032 
00033 AccelPlan::AccelPlan(art_pilot::PilotConfig &config):
00034   AccelBase(config),
00035   braking_(true),                       // begin with brake on
00036   brake_pid_(new Pid("brake", config.brake_kp, config.brake_ki,
00037                      config.brake_kd, 1.0, 0.0, 5000.0)),
00038   throttle_pid_(new Pid("throttle", config.throttle_kp, config.throttle_ki,
00039                         config.throttle_kd, 0.4, 0.0, 5000.0))
00040 {
00041   reset();
00042 };
00043 
00044 AccelPlan::~AccelPlan() {};
00045 
00046 void AccelPlan::adjust(art_msgs::PilotState &pstate,
00047                        ServoPtr brake, ServoPtr throttle)
00048 {
00049   // check whether target has changed since previous cycle
00050   float goal_speed = fabs(pstate.target.speed); 
00051   float goal_accel = fabs(pstate.target.acceleration);
00052   if (goal_speed != speed_ || goal_accel !=accel_) // target changed?
00053     {
00054       // save new targets for later comparisons
00055       speed_ = goal_speed;
00056       accel_ = goal_accel;
00057 
00058       // make a new acceleration plan
00059       pstate.plan.acceleration = accel_;
00060       if (accel_ == 0.0)                // no acceleration limit?
00061         {
00062           // plan directly to target speed
00063           pstate.plan.speed = speed_;
00064         }
00065       else
00066         {
00067           // make plan starting with current speed
00068           pstate.plan.speed = fabs(pstate.current.speed);
00069         }
00070     }
00071 
00072   // compute time since previous cycle
00073   float dt;                             // delta T
00074   if (prev_cycle_ == ros::Time())       // first cycle since reset?
00075     {
00076       // assume nominal cycle time
00077       dt = 1.0 / art_msgs::ArtHertz::PILOT;
00078     }
00079   else
00080     {
00081       // use actual time since previous cycle
00082       dt = (pstate.header.stamp - prev_cycle_).toSec();
00083     }
00084   
00085   // update plan in pilot state message
00086   if (accel_ != 0.0)                    // have acceleration limit?
00087     {
00088       // gradually change planned speed
00089       float error = speed_ - pstate.plan.speed;
00090       float dv = accel_ * dt;           // desired delta V for this cycle
00091 
00092       // limit error term to absolute value of requested acceleration
00093       if (fabs(error) <= dv)
00094         {
00095           pstate.plan.speed = speed_;
00096         }
00097       else
00098         {
00099           pstate.plan.speed += dv * signum(error);
00100         }
00101     }
00102 
00103   // request brake or throttle update to achieve planned velocity
00104   adjustVelocity(pstate, brake, throttle);
00105 
00106   // remember time of this cycle
00107   prev_cycle_ = pstate.header.stamp;
00108 }
00109 
00110 void AccelPlan::adjustVelocity(art_msgs::PilotState &pstate,
00111                                ServoPtr brake, ServoPtr throttle)
00112 {
00113   float brake_request;
00114   float throttle_request;
00115   float abs_speed = fabs(pstate.current.speed);
00116   float error = fabs(pstate.plan.speed) - abs_speed;
00117 
00118   if (braking_)
00119     {
00120       // controlling with brake:
00121       brake_request = brake_pid_->Update(error, abs_speed);
00122       throttle_request = 0.0;
00123       
00124       // If requesting brake off, switch to throttle control.
00125 
00126       // Must check reported brake position, too.  Otherwise there
00127       // will be considerable overlap, applying throttle while the
00128       // brake is still on.  That can cause mechanical damage to the
00129       // transmission.
00130       if ((brake->value() < Epsilon::brake_position)
00131           && (brake_request < Epsilon::brake_position))
00132         {
00133           brake_request = 0.0;          // brake off
00134           braking_ = false;             // using throttle now
00135           throttle_pid_->Clear();       // reset PID controller
00136         }
00137     }
00138   else
00139     {
00140       // controlling with throttle:
00141       throttle_request = throttle_pid_->Update(error, abs_speed);
00142       brake_request = 0.0;
00143 
00144       // If requesting throttle off, switch to brake control.
00145 
00146       // Since throttle responds much faster than brake, it will reach
00147       // idle before the brake really starts engaging.  So, not
00148       // checking throttle->value() here is an option, which reduces
00149       // latency when slowing down.
00150       if (throttle_request < Epsilon::throttle_position)
00151         {
00152           throttle_request = 0.0;       // throttle off
00153           braking_ = true;              // using brake now
00154           brake_pid_->Clear();          // reset PID controller
00155         }
00156     }
00157 
00158   brake_request = clamp(0.0, brake_request, 1.0);
00159   if (fabsf(brake_request - brake->value()) > Epsilon::brake_position)
00160     {
00161       brake->publish(brake_request, pstate.header.stamp);
00162     }
00163 
00164   throttle_request = clamp(0.0, throttle_request, 1.0);
00165   if (fabsf(throttle_request - throttle->value()) > Epsilon::throttle_position)
00166     {
00167       throttle->publish(throttle_request, pstate.header.stamp);
00168     }
00169 }
00170 
00172 void AccelPlan::reconfigure(art_pilot::PilotConfig &newconfig)
00173 {
00174   brake_pid_->Configure(newconfig.brake_kp,
00175                         newconfig.brake_ki,
00176                         newconfig.brake_kd);
00177   throttle_pid_->Configure(newconfig.throttle_kp,
00178                            newconfig.throttle_ki,
00179                            newconfig.throttle_kd);
00180 }
00181 
00183 void AccelPlan::reset(void)
00184 {
00185   // clear any existing acceleration plan
00186   prev_cycle_ == ros::Time();
00187   accel_ = 0.0;
00188   speed_ = 0.0;
00189 
00190   // clear any Ki build-up in PID controllers
00191   brake_pid_->Clear();
00192   throttle_pid_->Clear();
00193 }
00194 
00195 };
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends Defines


art_pilot
Author(s): Austin Robot Technology, Jack O'Quin
autogenerated on Tue Sep 24 2013 10:44:02