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 }\r
  16 ETBalanceRunner::~ETBalanceRunner(void)\r
  17 {\r
  18 }\r
  19 void ETBalanceRunner::Run(int forward, int turn)\r
  20 {\r
  21         S8 r,l;\r
  22         balance_control(forward,turn,m_gyro.getAnglerVelocity(),m_gyrooffset,m_motorL.getCount(),m_motorR.getCount(),m_nxt.getBattMv(),&l,&r);\r
  23         m_motorL.setPWM(l);\r
  24         m_motorR.setPWM(r);\r
  25 }\r
  26 void ETBalanceRunner::SetGyroOffset(int offset)\r
  27 {\r
  28         m_gyrooffset = offset;\r
  29 }\r
  30 \r
  31 }\r