source/libr2c2/simplecontroller.cpp

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