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