vehicle_platform/bcar.cc

   1 /*
   2 This file is part of I am car.
   3
   4 I am car is free software: you can redistribute it and/or modify
   5 it under the terms of the GNU General Public License as published by
   6 the Free Software Foundation, either version 3 of the License, or
   7 (at your option) any later version.
   8
   9 I am car is distributed in the hope that it will be useful,
  10 but WITHOUT ANY WARRANTY; without even the implied warranty of
  11 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12 GNU General Public License for more details.
  13
  14 You should have received a copy of the GNU General Public License
  15 along with I am car. If not, see <http://www.gnu.org/licenses/>.
  16 */
  17
  18 #include <cmath>
  19 #include "bcar.h"
  20
  21 float BicycleCar::speed()
  22 {
  23         return this->speed_;
  24 }
  25
  26 float BicycleCar::steer()
  27 {
  28         return this->steer_;
  29 }
  30
  31 bool BicycleCar::speed(float s)
  32 {
  33         this->speed_ = s;
  34         return true;
  35 }
  36
  37 bool BicycleCar::steer(float s)
  38 {
  39         this->steer_ = s;
  40         return true;
  41 }
  42
  43 std::vector<RRTEdge *> BicycleCar::frame()
  44 {
  45         std::vector<RRTEdge *> frame;
  46         float dr = (this->length_ - this->wheel_base_) / 2;
  47         float df = this->length_ - dr;
  48
  49         float lfx = this->x();
  50         lfx += (this->width_ / 2) * cos(this->h() + M_PI / 2);
  51         lfx += df * cos(this->h());
  52         lfx += this->safety_dist_ * cos(this->h());
  53
  54         float lrx = this->x();
  55         lrx += (this->width_ / 2) * cos(this->h() + M_PI / 2);
  56         lrx += -dr * cos(this->h());
  57         lrx += -this->safety_dist_ * cos(this->h());
  58
  59         float rrx = this->x();
  60         rrx += (this->width_ / 2) * cos(this->h() - M_PI / 2);
  61         rrx += -dr * cos(this->h());
  62         rrx += -this->safety_dist_ * cos(this->h());
  63
  64         float rfx = this->x();
  65         rfx += (this->width_ / 2) * cos(this->h() - M_PI / 2);
  66         rfx += df * cos(this->h());
  67         rfx += this->safety_dist_ * cos(this->h());
  68
  69         float lfy = this->y();
  70         lfy += (this->width_ / 2) * sin(this->h() + M_PI / 2);
  71         lfy += df * sin(this->h());
  72         lfy += this->safety_dist_ * sin(this->h());
  73
  74         float lry = this->y();
  75         lry += (this->width_ / 2) * sin(this->h() + M_PI / 2);
  76         lry += -dr * sin(this->h());
  77         lry += -this->safety_dist_ * sin(this->h());
  78
  79         float rry = this->y();
  80         rry += (this->width_ / 2) * sin(this->h() - M_PI / 2);
  81         rry += -dr * sin(this->h());
  82         rry += -this->safety_dist_ * sin(this->h());
  83
  84         float rfy = this->y();
  85         rfy += (this->width_ / 2) * sin(this->h() - M_PI / 2);
  86         rfy += df * sin(this->h());
  87         rfy += this->safety_dist_ * sin(this->h());
  88
  89         frame.push_back(new RRTEdge(
  90                                 new RRTNode(lfx, lfy),
  91                                 new RRTNode(lrx, lry)));
  92         frame.push_back(new RRTEdge(
  93                                 new RRTNode(lrx, lry),
  94                                 new RRTNode(rrx, rry)));
  95         frame.push_back(new RRTEdge(
  96                                 new RRTNode(rrx, rry),
  97                                 new RRTNode(rfx, rfy)));
  98         frame.push_back(new RRTEdge(
  99                                 new RRTNode(rfx, rfy),
 100                                 new RRTNode(lfx, rfy)));
 101         return frame;
 102 }
 103
 104 bool BicycleCar::next()
 105 {
 106         if (this->steer_ > MAXSTEER(this->wheel_base_, this->turning_radius_))
 107                 this->steer_ = MAXSTEER(
 108                                 this->wheel_base_,
 109                                 this->turning_radius_);
 110         if (this->steer_ < -MAXSTEER(this->wheel_base_, this->turning_radius_))
 111                 this->steer_ = -MAXSTEER(
 112                                 this->wheel_base_,
 113                                 this->turning_radius_);
 114         this->h_ += (this->speed_ / this->wheel_base_) * tan(this->steer_);
 115         this->x_ += this->speed_ * cos(this->h_);
 116         this->y_ += this->speed_ * sin(this->h_);
 117         return true;
 118 }
 119
 120 bool BicycleCar::next(float speed, float steer)
 121 {
 122         this->speed_ = speed;
 123         this->steer_ = steer;
 124         return this->next();
 125 }