ETBalanceRunner.cpp

   1 #include "ETBalanceRunner.h"\r
   2 #include "Motor.h"\r
   3 #include "GyroSensor.h"\r
   4 #include "Nxt.h"\r
   5 \r
   6 extern "C" {\r
   7 #include "balancer.h"\r
   8 }\r
   9 \r
  10 namespace ecrobot{\r
  11 \r
  12 ETBalanceRunner::ETBalanceRunner(Motor& motorL, Motor& motorR, GyroSensor& gyro, Nxt& nxt)\r
  13         : m_motorL(motorL), m_motorR(motorR), m_gyro(gyro), m_nxt(nxt), m_gyrooffset(610), m_bException(false), m_msec(0)\r
  14 {\r
  15         balance_init();\r
  16 }\r
  17 ETBalanceRunner::~ETBalanceRunner(void)\r
  18 {\r
  19 }\r
  20 void ETBalanceRunner::Run(int forward, int turn)\r
  21 {\r
  22         S8 r,l;\r
  23         balance_control(forward,0,m_gyro.get(),m_gyrooffset,m_motorL.getCount(),m_motorR.getCount(),m_nxt.getBattMv(),&l,&r);\r
  24         JudgeException(l,r);\r
  25         if(IsException()){\r
  26                 Stop();\r
  27                 return;\r
  28         }\r
  29         l = l + turn;\r
  30         r = r - turn;\r
  31         if(100 < l){\r
  32                 r = r + (l - 100);\r
  33                 l = 100;\r
  34         }else if(r < -100){\r
  35                 l = l + (r - (-100));\r
  36                 r = -100;\r
  37         }\r
  38         m_motorL.setPWM(l);\r
  39         m_motorR.setPWM(r);\r
  40 }\r
  41 bool ETBalanceRunner::IsException() const\r
  42 {\r
  43         return m_bException;\r
  44 }\r
  45 void ETBalanceRunner::Stop()\r
  46 {\r
  47         m_motorL.setPWM(0);\r
  48         m_motorR.setPWM(0);\r
  49 }\r
  50 void ETBalanceRunner::Reset()\r
  51 {\r
  52         balance_init();\r
  53         m_bException = false;\r
  54         m_msec = 0;\r
  55 }\r
  56 void ETBalanceRunner::SetGyroOffset(int offset)\r
  57 {\r
  58         m_gyrooffset = offset;\r
  59 }\r
  60 void ETBalanceRunner::JudgeException(int l,int r)\r
  61 {\r
  62         static const int LIMIT = 2000;  /* 2[sec] */\r
  63         if((l == 100 && r == 100) || (l == -100 && r == -100)){ /* +100\82©-100\82É\92£\82è\95t\82« */\r
  64                 if(m_msec >= LIMIT){    /* \88ê\92è\8e\9e\8aÔ\81}100\82ð\88Û\8e\9d\82µ\82½\82ç\97á\8aO */\r
  65                         m_bException = true;\r
  66                 }\r
  67                 else{\r
  68                         m_msec += 4;    /* +4[msec] */\r
  69                 }\r
  70         }else{\r
  71                 m_msec = 0;\r
  72         }\r
  73 }\r
  74 \r
  75 }\r