ETTailControl.cpp

   1 #include "ETTailControl.h"\r
   2 #include "Motor.h"\r
   3 \r
   4 using namespace ecrobot;\r
   5 \r
   6 extern "C"\r
   7 {\r
   8         #include "ecrobot_interface.h"\r
   9 };\r
  10 \r
  11 const int ETTailControl::ANGLE_STAND_UP = 115;\r
  12 const int ETTailControl::ANGLE_DRIVE = 3;\r
  13 const float ETTailControl::P_GAIN = 2.5F;\r
  14 const int ETTailControl::PWM_ABS_MAX = 60;\r
  15 const int ETTailControl::RESET_SPEED = -50;\r
  16 const int ETTailControl::RESET_PROCESSING_INTERVAL = 64;\r
  17 \r
  18 /* \8f\89\8aú\89»\82·\82é */\r
  19 void ETTailControl::Init()\r
  20 {\r
  21         /* \82µ\82Á\82Û\82ð\8aª\82«\82 \82°\82Ä\93®\82©\82È\82\82È\82Á\82½\82ç\81C\89ñ\93]\8ap\93x\82ð\83\8a\83Z\83b\83g\82·\82é */\r
  22         m_tailMotor.reset();\r
  23         long currentDegree = 0;\r
  24         long lastDegree = 0;\r
  25         \r
  26         do {\r
  27                 lastDegree = currentDegree;\r
  28                 m_tailMotor.setPWM(RESET_SPEED);\r
  29                 systick_wait_ms(RESET_PROCESSING_INTERVAL);\r
  30 \r
  31                 currentDegree = m_tailMotor.getCount();\r
  32         } while(currentDegree < lastDegree);\r
  33 \r
  34         m_angle = 0;\r
  35         m_tailMotor.reset();\r
  36 }\r
  37 \r
  38 /* \90K\94ö\90§\8cä\82ð\8ds\82¤ */\r
  39 void ETTailControl::Control()\r
  40 {\r
  41         float pwm = (static_cast<float>(m_angle) - static_cast<float>(m_tailMotor.getCount()))*P_GAIN;  /* \94ä\97á\90§\8cä */\r
  42         /* PWM\8fo\97Í\96O\98a\8f\88\97\9d */\r
  43         if (pwm > PWM_ABS_MAX)\r
  44         {\r
  45                 pwm = PWM_ABS_MAX;\r
  46         }\r
  47         else if (pwm < -PWM_ABS_MAX)\r
  48         {\r
  49                 pwm = -PWM_ABS_MAX;\r
  50         }\r
  51 \r
  52         m_tailMotor.setPWM((signed char)pwm);\r
  53 }\r
  54 \r