source/libr2c2/simplecontroller.cpp

   1 #include <msp/core/maputils.h>
   2 #include <msp/time/units.h>
   3 #include "simplecontroller.h"
   4
   5 using namespace std;
   6 using namespace Msp;
   7
   8 namespace R2C2 {
   9
  10 SimpleController::SimpleController():
  11         target_speed(Control::continuous("speed", 0, 1000)),
  12         reverse(Control::binary("reverse")),
  13         accel(0.07),
  14         speed(0)
  15 {
  16         target_speed.set(0);
  17 }
  18
  19 const char *SimpleController::enumerate_controls(unsigned index) const
  20 {
  21         if(index==0)
  22                 return target_speed.name.c_str();
  23         else if(index==1)
  24                 return reverse.name.c_str();
  25         else
  26                 return 0;
  27 }
  28
  29 void SimpleController::set_control(const string &name, float v)
  30 {
  31         if(name==target_speed.name)
  32         {
  33                 target_speed.set(v);
  34                 signal_control_changed.emit(target_speed);
  35         }
  36         else if(name==reverse.name)
  37         {
  38                 if(target_speed.value || speed)
  39                         throw logic_error("Must be stopped to change reverse");
  40                 reverse.set(v);
  41                 signal_control_changed.emit(reverse);
  42         }
  43 }
  44
  45 const Controller::Control &SimpleController::get_control(const string &name) const
  46 {
  47         if(name==target_speed.name)
  48                 return target_speed;
  49         else if(name==reverse.name)
  50                 return reverse;
  51         else
  52                 throw key_error(name);
  53 }
  54
  55 float SimpleController::get_braking_distance() const
  56 {
  57         return speed*speed/(2*accel);
  58 }
  59
  60 void SimpleController::tick(const Time::TimeDelta &dt)
  61 {
  62         float secs = dt/Time::sec;
  63         if(speed<target_speed.value)
  64                 speed = min(speed+secs*accel, target_speed.value);
  65         else if(speed>target_speed.value)
  66                 speed = max(speed-secs*accel, target_speed.value);
  67 }
  68
  69 } // namespace R2C2