尻尾制御クラスを追加した。

diff --git a/ETTailControl.cpp b/ETTailControl.cpp
new file mode 100644 (file)
index 0000000..89421d7
--- /dev/null
+++ b/ETTailControl.cpp
@@ -0,0 +1,54 @@
+#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

diff --git a/ETTailControl.h b/ETTailControl.h
new file mode 100644 (file)
index 0000000..83a1504
--- /dev/null
+++ b/ETTailControl.h
@@ -0,0 +1,22 @@
+#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

diff --git a/Makefile b/Makefile
index 7a13a1e..d51ea17 100644 (file)
--- a/Makefile
+++ b/Makefile
@@ -5,6 +5,7 @@ TARGET_CPP_SOURCES = main.cpp LineTracer.cpp LinePos.cpp Runner.cpp ETLinePos.cp
 context.cpp \
 runstate.cpp \
 exceptionstate.cpp \
+ETTailControl.cpp \
 
 
 TARGET_SOURCES = tsprintf.c balancer_param.c

diff --git a/context.h b/context.h
index 71e1b5a..fdc84e2 100644 (file)
--- a/context.h
+++ b/context.h
@@ -23,6 +23,6 @@ namespace ContextStuff{
                void Display(ecrobot::Lcd& lcd);\r
         void SetETLineTracer(ETLineTracer* value);\r
        };\r
-};\r
+}\r
 \r
 #endif /* CONTEXT_H */\r

diff --git a/main.cpp b/main.cpp
index 2f1eca6..b605872 100644 (file)
--- a/main.cpp
+++ b/main.cpp
@@ -16,6 +16,7 @@
 #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
@@ -45,6 +46,7 @@ Motor motorL(PORT_C);
 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
@@ -65,6 +67,9 @@ TASK(TaskMain)
        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
@@ -72,6 +77,7 @@ TASK(TaskMain)
        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
@@ -89,6 +95,7 @@ TASK(TaskMain)
 \r
        // \94\92\r
        while(!touch.isPressed()){\r
+        tailcontroler.Control();\r
        }\r
        while(touch.isPressed()){\r
        }\r
@@ -107,6 +114,7 @@ TASK(TaskMain)
 \r
        // \8d\95\r
        while(!touch.isPressed()){\r
+        tailcontroler.Control();\r
        }\r
        while(touch.isPressed()){\r
        }\r
@@ -124,6 +132,7 @@ TASK(TaskMain)
        speaker.playTone(1400,100,50);\r
 \r
        while(!touch.isPressed()){\r
+        tailcontroler.Control();\r
        }\r
        while(touch.isPressed()){\r
        }\r