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)\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         l = l + turn;\r
  25         r = r - turn;\r
  26         if(100 < l){\r
  27                 r = r + (l - 100);\r
  28                 l = 100;\r
  29         }else if(r < -100){\r
  30                 l = l + (r - (-100));\r
  31                 r = -100;\r
  32         }\r
  33         m_motorL.setPWM(l);\r
  34         m_motorR.setPWM(r);\r
  35 }\r
  36 bool ETBalanceRunner::IsException() const\r
  37 {\r
  38         return false;\r
  39 }\r
  40 void ETBalanceRunner::Stop()\r
  41 {\r
  42         m_motorL.setPWM(0);\r
  43         m_motorR.setPWM(0);\r
  44 }\r
  45 void ETBalanceRunner::Reset()\r
  46 {\r
  47         balance_init();\r
  48 }\r
  49 void ETBalanceRunner::SetGyroOffset(int offset)\r
  50 {\r
  51         m_gyrooffset = offset;\r
  52 }\r
  53 \r
  54 }\r