--- /dev/null
+#include "ETTailControl.h"\r
+#include "Motor.h"\r
+\r
+using namespace ecrobot;\r
+\r
+extern "C"\r
+{\r
+ #include "ecrobot_interface.h"\r
+};\r
+\r
+const int ETTailControl::ANGLE_STAND_UP = 115;\r
+const int ETTailControl::ANGLE_DRIVE = 3;\r
+const float ETTailControl::P_GAIN = 2.5F;\r
+const int ETTailControl::PWM_ABS_MAX = 60;\r
+const int ETTailControl::RESET_SPEED = -50;\r
+const int ETTailControl::RESET_PROCESSING_INTERVAL = 64;\r
+\r
+/* \8f\89\8aú\89»\82·\82é */\r
+void ETTailControl::Init()\r
+{\r
+ /* \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
+ m_tailMotor.reset();\r
+ long currentDegree = 0;\r
+ long lastDegree = 0;\r
+ \r
+ do {\r
+ lastDegree = currentDegree;\r
+ m_tailMotor.setPWM(RESET_SPEED);\r
+ systick_wait_ms(RESET_PROCESSING_INTERVAL);\r
+\r
+ currentDegree = m_tailMotor.getCount();\r
+ } while(currentDegree < lastDegree);\r
+\r
+ m_angle = 0;\r
+ m_tailMotor.reset();\r
+}\r
+\r
+/* \90K\94ö\90§\8cä\82ð\8ds\82¤ */\r
+void ETTailControl::Control()\r
+{\r
+ float pwm = (static_cast<float>(m_angle) - static_cast<float>(m_tailMotor.getCount()))*P_GAIN; /* \94ä\97á\90§\8cä */\r
+ /* PWM\8fo\97Í\96O\98a\8f\88\97\9d */\r
+ if (pwm > PWM_ABS_MAX)\r
+ {\r
+ pwm = PWM_ABS_MAX;\r
+ }\r
+ else if (pwm < -PWM_ABS_MAX)\r
+ {\r
+ pwm = -PWM_ABS_MAX;\r
+ }\r
+\r
+ m_tailMotor.setPWM((signed char)pwm);\r
+}\r
+\r
--- /dev/null
+#pragma once\r
+\r
+namespace ecrobot{\r
+ class Motor;\r
+ class ETTailControl\r
+ {\r
+ int m_angle;\r
+ Motor& m_tailMotor;\r
+ public:\r
+ ETTailControl(Motor& tailMotor,int angle):m_angle(angle),m_tailMotor(tailMotor){;}\r
+ ~ETTailControl(){;}\r
+ void Init();\r
+ void Control();\r
+ void SetAngle(const int angle){m_angle = angle;}\r
+ static const int ANGLE_STAND_UP; /* \8a®\91S\92â\8e~\8e\9e\82Ì\8ap\93x[\93x] */\r
+ static const int ANGLE_DRIVE; /* \83o\83\89\83\93\83X\91\96\8ds\8e\9e\82Ì\8ap\93x[\93x] */\r
+ static const float P_GAIN; /* \8a®\91S\92â\8e~\97p\83\82\81[\83^\90§\8cä\94ä\97á\8cW\90\94 */\r
+ static const int PWM_ABS_MAX; /* \8a®\91S\92â\8e~\97p\83\82\81[\83^\90§\8cäPWM\90â\91Î\8dÅ\91å\92l */\r
+ static const int RESET_SPEED;\r
+ static const int RESET_PROCESSING_INTERVAL; /* msec */\r
+ };\r
+}\r
context.cpp \
runstate.cpp \
exceptionstate.cpp \
+ETTailControl.cpp \
TARGET_SOURCES = tsprintf.c balancer_param.c
void Display(ecrobot::Lcd& lcd);\r
void SetETLineTracer(ETLineTracer* value);\r
};\r
-};\r
+}\r
\r
#endif /* CONTEXT_H */\r
#include "ETLineTracer.h"\r
#include "ETLinePos.h"\r
#include "ETBalanceRunner.h"\r
+#include "ETTailControl.h"\r
#include "context.h"\r
\r
using namespace ecrobot;\r
ETBalanceRunner runner(motorL,motorR,gyro,nxt);\r
ETLinePos linepos(light);\r
ETLineTracer tracer(linepos,runner);\r
+ETTailControl tailcontroler(motorT,0);\r
ContextStuff::Context context;\r
\r
// nxtOSEK hook to be invoked from an ISR in category 2\r
lcd.clear();\r
lcd.putf("s", "Hello World");\r
lcd.disp();\r
+ tailcontroler.Init();\r
+ tailcontroler.SetAngle(ETTailControl::ANGLE_STAND_UP);\r
+\r
\r
// TODO:\82Æ\82è\82 \82¦\82¸\82Ì\83L\83\83\83\8a\83u\83\8c\81[\83V\83\87\83\93\r
static const int sum_num = 256;\r
char str[16+1];\r
// \83W\83\83\83C\83\8d\r
while(!touch.isPressed()){\r
+ tailcontroler.Control();\r
}\r
while(touch.isPressed()){\r
}\r
\r
// \94\92\r
while(!touch.isPressed()){\r
+ tailcontroler.Control();\r
}\r
while(touch.isPressed()){\r
}\r
\r
// \8d\95\r
while(!touch.isPressed()){\r
+ tailcontroler.Control();\r
}\r
while(touch.isPressed()){\r
}\r
speaker.playTone(1400,100,50);\r
\r
while(!touch.isPressed()){\r
+ tailcontroler.Control();\r
}\r
while(touch.isPressed()){\r
}\r