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::dr()
  22 {
  23         return (this->length_ - this->wheel_base_) / 2;
  24 }
  25
  26 float BicycleCar::df()
  27 {
  28         return this->length_ - this->dr();
  29 }
  30
  31 float BicycleCar::lfx()
  32 {
  33
  34         float lfx = this->x();
  35         lfx += (this->width_ / 2) * cos(this->h() + M_PI / 2);
  36         lfx += this->df() * cos(this->h());
  37         lfx += this->safety_dist_ * cos(this->h());
  38         return lfx;
  39 }
  40
  41 float BicycleCar::lrx()
  42 {
  43         float lrx = this->x();
  44         lrx += (this->width_ / 2) * cos(this->h() + M_PI / 2);
  45         lrx += -this->dr() * cos(this->h());
  46         lrx += -this->safety_dist_ * cos(this->h());
  47         return lrx;
  48 }
  49
  50 float BicycleCar::rrx()
  51 {
  52         float rrx = this->x();
  53         rrx += (this->width_ / 2) * cos(this->h() - M_PI / 2);
  54         rrx += -this->dr() * cos(this->h());
  55         rrx += -this->safety_dist_ * cos(this->h());
  56         return rrx;
  57 }
  58
  59 float BicycleCar::rfx()
  60 {
  61         float rfx = this->x();
  62         rfx += (this->width_ / 2) * cos(this->h() - M_PI / 2);
  63         rfx += this->df() * cos(this->h());
  64         rfx += this->safety_dist_ * cos(this->h());
  65         return rfx;
  66 }
  67
  68 float BicycleCar::lfy()
  69 {
  70         float lfy = this->y();
  71         lfy += (this->width_ / 2) * sin(this->h() + M_PI / 2);
  72         lfy += this->df() * sin(this->h());
  73         lfy += this->safety_dist_ * sin(this->h());
  74         return lfy;
  75 }
  76
  77 float BicycleCar::lry()
  78 {
  79         float lry = this->y();
  80         lry += (this->width_ / 2) * sin(this->h() + M_PI / 2);
  81         lry += -this->dr() * sin(this->h());
  82         lry += -this->safety_dist_ * sin(this->h());
  83         return lry;
  84 }
  85
  86 float BicycleCar::rry()
  87 {
  88         float rry = this->y();
  89         rry += (this->width_ / 2) * sin(this->h() - M_PI / 2);
  90         rry += -this->dr() * sin(this->h());
  91         rry += -this->safety_dist_ * sin(this->h());
  92         return rry;
  93 }
  94
  95 float BicycleCar::rfy()
  96 {
  97         float rfy = this->y();
  98         rfy += (this->width_ / 2) * sin(this->h() - M_PI / 2);
  99         rfy += this->df() * sin(this->h());
 100         rfy += this->safety_dist_ * sin(this->h());
 101         return rfy;
 102 }
 103
 104 float BicycleCar::speed()
 105 {
 106         return this->speed_;
 107 }
 108
 109 float BicycleCar::steer()
 110 {
 111         return this->steer_;
 112 }
 113
 114 bool BicycleCar::speed(float s)
 115 {
 116         this->speed_ = s;
 117         return true;
 118 }
 119
 120 bool BicycleCar::steer(float s)
 121 {
 122         this->steer_ = s;
 123         return true;
 124 }
 125
 126 std::vector<RRTEdge *> BicycleCar::frame()
 127 {
 128         std::vector<RRTEdge *> frame;
 129         frame.push_back(new RRTEdge(
 130                                 new RRTNode(this->lfx(), this->lfy()),
 131                                 new RRTNode(this->lrx(), this->lry())));
 132         frame.push_back(new RRTEdge(
 133                                 new RRTNode(this->lrx(), this->lry()),
 134                                 new RRTNode(this->rrx(), this->rry())));
 135         frame.push_back(new RRTEdge(
 136                                 new RRTNode(this->rrx(), this->rry()),
 137                                 new RRTNode(this->rfx(), this->rfy())));
 138         frame.push_back(new RRTEdge(
 139                                 new RRTNode(this->rfx(), this->rfy()),
 140                                 new RRTNode(this->lfx(), this->rfy())));
 141         return frame;
 142 }
 143
 144 bool BicycleCar::next()
 145 {
 146         if (this->steer_ > MAXSTEER(this->wheel_base_, this->turning_radius_))
 147                 this->steer_ = MAXSTEER(
 148                                 this->wheel_base_,
 149                                 this->turning_radius_);
 150         if (this->steer_ < -MAXSTEER(this->wheel_base_, this->turning_radius_))
 151                 this->steer_ = -MAXSTEER(
 152                                 this->wheel_base_,
 153                                 this->turning_radius_);
 154         this->h_ += (this->speed_ / this->wheel_base_) * tan(this->steer_);
 155         this->x_ += this->speed_ * cos(this->h_);
 156         this->y_ += this->speed_ * sin(this->h_);
 157         return true;
 158 }
 159
 160 bool BicycleCar::next(float speed, float steer)
 161 {
 162         this->speed_ = speed;
 163         this->steer_ = steer;
 164         return this->next();
 165 }