speed.cc
Go to the documentation of this file.
00001 /*
00002  *  Copyright (C) 2005, 2007, 2009, 2011 Austin Robot Technology
00003  *  License: Modified BSD Software License Agreement
00004  * 
00005  *  $Id: speed.cc 1539 2011-05-09 04:09:20Z jack.oquin $
00006  */
00007 
00015 #include <art/conversions.h>
00016 #include <art_msgs/ArtHertz.h>
00017 #include "speed.h"
00018 
00023 namespace ControlMatrix
00024 {
00025 
00026   // acceleration matrix dimensions
00027   static const int N_DELTAS = 13;
00028   static const int DELTA_0 = (N_DELTAS-1)/2; // middle row: delta == 0
00029   static const int N_SPEEDS = 6;
00030 
00031   // Velocity control
00032   typedef struct
00033   {
00034     float       brake_delta;
00035     float       throttle_delta;
00036   } accel_parms_t;
00037 
00038   // Acceleration matrix: rows are indexed according to the requested
00039   // speed delta in MPH, columns are indexed by current speed in MPH.
00040   // The main reason for making all this table-driven is so we can
00041   // adjust some values without affecting all the others.  
00042   //
00043   // NOTE: values in this table represent a percentage change per second,
00044   // so their effect builds up quickly.
00045 
00046   static const accel_parms_t accel_matrix[N_DELTAS][N_SPEEDS] =
00047     { /*              0        <=4       <=8       <=16      <=32      more  */
00048       /* -32 */ {{1000,-90}, {12,-30}, {12,-30}, {12,-40}, {12,-50}, {20,-70}},
00049       /* -16 */ {{1000,-60}, { 6,-20}, { 6,-20}, { 6,-30}, { 6,-40}, {10,-55}},
00050       /*  -8 */ {{1000,-40}, { 4,-15}, { 4,-15}, { 4,-20}, { 4,-30}, { 7,-40}},
00051       /*  -4 */ { {500,-20}, { 3,-10}, { 3,-10}, { 3,-10}, { 3,-20}, { 5,-25}},
00052       /*  -2 */ { {200,-10},  {2,-5},   {2,-5},   {2,-5},   {2,-10},  {3,-10}},
00053       /*  -1 */ { {100,-5},   {1,-2},   {1,-2},   {1,-2},   {1,-5},   {1,-5} },
00054       /*   0 */ { {100,0},    {0,0},    {0,0},    {0,0},    {0,0},    {0,0}  },
00055       /*   1 */ {{-100,1},    {-2,2},   {-2,2},   {-2,2},   {-2,5},   {-2,5} },
00056       /*   2 */ {{-200,2},    {-5,5},   {-5,5},   {-5,5},   {-5,10},  {-5,10}},
00057       /*   4 */ {{-300,3},   {-10,10}, {-10,10}, {-10,10}, {-10,20}, {-10,25}},
00058       /*   8 */ {{-500,4},   {-15,15}, {-15,15}, {-15,20}, {-15,30}, {-15,40}},
00059       /*  16 */ {{-1000,5},  {-20,20}, {-20,20}, {-20,30}, {-20,40}, {-20,55}},
00060       /*  32 */ {{-1000,6},  {-30,30}, {-30,30}, {-30,40}, {-30,50}, {-30,70}}
00061     };
00062 
00063   // If it turns out that the matrix indices should be different, adjust
00064   // the conversions in these functions.  The logarithmic intervals are
00065   // based on intuition.
00066   static inline int
00067   delta_row(float mph)
00068   {
00069     int row;
00070     static float row_limits[DELTA_0+1] = {0.0, 1.0, 2.0, 4.0, 8.0, 16.0, 32.0};
00071     int mph_sign = 1;
00072 
00073     if (mph < 0)
00074       mph_sign = -1;
00075 
00076     float abs_mph = mph * mph_sign;
00077 
00078     for (row = 0; row < DELTA_0+1; ++row)
00079       {
00080         if (abs_mph <= row_limits[row])
00081           return DELTA_0 + mph_sign * row;
00082       }
00083 
00084     // if delta outside the table, use the first or last entry
00085     return DELTA_0 + mph_sign * DELTA_0;
00086   }
00087 
00088   static inline int
00089   speed_col(float mph)
00090   {
00091     int col;
00092     static float col_limits[N_SPEEDS] = {0.0, 4.0, 8.0, 16.0, 32.0, 64.0};
00093 
00094     for (col = 0; col < N_SPEEDS; ++col)
00095       {
00096         if (mph <= col_limits[col])
00097           return col;
00098       }
00099 
00100     // if speed beyond the table, use the last entry
00101     return N_SPEEDS-1;
00102   }
00103 };
00104 
00106 SpeedControlMatrix::SpeedControlMatrix():
00107   SpeedControl(),
00108   velpid_(new Pid("speed", 2.0, 0.0, 32.0))
00109 {
00110   // Allocate speed PID class with default parameters, then
00111   // configure control constants.
00112   //
00113   // Default PD parameters: make Kd proportional to cycle rate, higher
00114   // frequencies will yield lower deltaV per cycle.
00115   //
00116   reset();
00117 }
00118 
00120 SpeedControlMatrix::~SpeedControlMatrix()
00121 {}
00122 
00135 void SpeedControlMatrix::adjust(float speed, float error,
00136                                 float *brake_req, float *throttle_req)
00137 {
00138   using namespace ControlMatrix;
00139 
00140   float delta = velpid_->Update(error, speed);
00141   float delta_mph = mps2mph(delta);
00142 
00143   // index into the acceleration matrix
00144   int row = delta_row(delta_mph);
00145   int col = speed_col(mps2mph(speed));
00146   float brake_delta =
00147     (accel_matrix[row][col].brake_delta / art_msgs::ArtHertz::PILOT)
00148     / 100.0;
00149   float throttle_delta =
00150     (accel_matrix[row][col].throttle_delta / art_msgs::ArtHertz::PILOT)
00151     / 100.0;
00152 
00153   ROS_DEBUG("accel_matrix[%d][%d] contains {%.3f, %.3f}",
00154             row, col, brake_delta, throttle_delta);
00155 
00156   // Do not add braking unless nearly idle throttle was previously
00157   // requested, or throttle unless the brake is nearly off.
00158   if (*throttle_req > 0.0 && brake_delta > 0.0)
00159     brake_delta = 0.0;
00160   if (*brake_req > 0.0 && throttle_delta > 0.0)
00161     throttle_delta = 0.0;
00162   *brake_req += brake_delta;
00163   *throttle_req += throttle_delta;
00164 }
00165 
00167 void SpeedControlMatrix::configure(art_pilot::PilotConfig &newconfig)
00168 {
00169   velpid_->Configure(node_);
00170 }
00171 
00173 void SpeedControlMatrix::reset(void)
00174 {
00175   velpid_->Clear();
00176 }
00177 
00186 SpeedControlPID::SpeedControlPID():
00187   SpeedControl(),
00188   braking_(true),                       // begin with brake on
00189   brake_pid_(new Pid("brake", -0.2, -0.0002, -1.6, 1.0, 0.0, 5000.0)),
00190   throttle_pid_(new Pid("throttle", 0.12, 0.001, 0.54, 0.4, 0.0, 5000.0))
00191 {
00192   reset();
00193 }
00194 
00196 SpeedControlPID::~SpeedControlPID()
00197 {};
00198 
00212 void SpeedControlPID::adjust(float speed, float error,
00213                              float *brake_req, float *throttle_req)
00214 {
00215   if (braking_)
00216     {
00217       // controlling with brake:
00218       *brake_req = brake_pid_->Update(error, speed);
00219       *throttle_req = 0.0;
00220       
00221       // If requesting brake off, switch to throttle control.
00222 #if 1
00223       // Must check reported brake position, too.  Otherwise there
00224       // will be considerable overlap, applying throttle while the
00225       // brake is still on.  That can cause mechanical damage to the
00226       // transmission.
00227       if ((brake_position_ < EPSILON_BRAKE) && (*brake_req < EPSILON_BRAKE))
00228         {
00229           *brake_req = 0.0;             // brake off
00230           braking_ = false;             // using throttle now
00231           throttle_pid_->Clear();       // reset PID controller
00232         }
00233 #else
00234       // Allow more overlap, to damp the oscillations that occur when
00235       // switching back and forth between throttle and brake.
00236       if ((*brake_req < EPSILON_BRAKE))
00237         {
00238           *brake_req = 0.0;             // brake off
00239           braking_ = false;             // using throttle now
00240           throttle_pid_->Clear();       // reset PID controller
00241         }
00242 #endif
00243     }
00244   else
00245     {
00246       // controlling with throttle:
00247       *throttle_req = throttle_pid_->Update(error, speed);
00248       *brake_req = 0.0;
00249 
00250       // If requesting throttle off, switch to brake control.
00251 
00252       // Since throttle responds much faster than brake, it will reach
00253       // idle before the brake really starts engaging.  So, not
00254       // checking throttle_position_ here is an option, which reduces
00255       // latency when slowing down.
00256       if (*throttle_req < EPSILON_THROTTLE)
00257         {
00258           *throttle_req = 0.0;          // throttle off
00259           braking_ = true;              // using brake now
00260           brake_pid_->Clear();          // reset PID controller
00261         }
00262     }
00263 }
00264 
00266 void SpeedControlPID::configure(art_pilot::PilotConfig &newconfig)
00267 {
00268   brake_pid_->Configure(newconfig.brake_kp,
00269                         newconfig.brake_ki,
00270                         newconfig.brake_kd);
00271   throttle_pid_->Configure(newconfig.throttle_kp,
00272                            newconfig.throttle_ki,
00273                            newconfig.throttle_kd);
00274 }
00275 
00277 void SpeedControlPID::reset(void)
00278 {
00279   brake_pid_->Clear();
00280   throttle_pid_->Clear();
00281 }
 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