balance_controlの出力が張り付いたときのフェールセーフを実装したつもり。

diff --git a/ETBalanceRunner.cpp b/ETBalanceRunner.cpp
index b227673..659d899 100644 (file)
--- a/ETBalanceRunner.cpp
+++ b/ETBalanceRunner.cpp
@@ -10,7 +10,7 @@ extern "C" {
 namespace ecrobot{\r
 \r
 ETBalanceRunner::ETBalanceRunner(Motor& motorL, Motor& motorR, GyroSensor& gyro, Nxt& nxt)\r
-       : m_motorL(motorL), m_motorR(motorR), m_gyro(gyro), m_nxt(nxt), m_gyrooffset(610)\r
+       : m_motorL(motorL), m_motorR(motorR), m_gyro(gyro), m_nxt(nxt), m_gyrooffset(610), m_bException(false), m_msec(0)\r
 {\r
        balance_init();\r
 }\r
@@ -21,6 +21,11 @@ void ETBalanceRunner::Run(int forward, int turn)
 {\r
        S8 r,l;\r
        balance_control(forward,0,m_gyro.get(),m_gyrooffset,m_motorL.getCount(),m_motorR.getCount(),m_nxt.getBattMv(),&l,&r);\r
+       JudgeException(l,r);\r
+       if(IsException()){\r
+               Stop();\r
+               return;\r
+       }\r
        l = l + turn;\r
        r = r - turn;\r
        if(100 < l){\r
@@ -35,7 +40,7 @@ void ETBalanceRunner::Run(int forward, int turn)
 }\r
 bool ETBalanceRunner::IsException() const\r
 {\r
-       return false;\r
+       return m_bException;\r
 }\r
 void ETBalanceRunner::Stop()\r
 {\r
@@ -45,10 +50,26 @@ void ETBalanceRunner::Stop()
 void ETBalanceRunner::Reset()\r
 {\r
        balance_init();\r
+       m_bException = false;\r
+       m_msec = 0;\r
 }\r
 void ETBalanceRunner::SetGyroOffset(int offset)\r
 {\r
        m_gyrooffset = offset;\r
 }\r
+void ETBalanceRunner::JudgeException(int l,int r)\r
+{\r
+       static const int LIMIT = 2000;  /* 2[sec] */\r
+       if((l == 100 && r == 100) || (l == -100 && r == -100)){ /* +100\82©-100\82É\92£\82è\95t\82« */\r
+               if(m_msec >= LIMIT){    /* \88ê\92è\8e\9e\8aÔ\81}100\82ð\88Û\8e\9d\82µ\82½\82ç\97á\8aO */\r
+                       m_bException = true;\r
+               }\r
+               else{\r
+                       m_msec += 4;    /* +4[msec] */\r
+               }\r
+       }else{\r
+               m_msec = 0;\r
+       }\r
+}\r
 \r
 }\r

diff --git a/ETBalanceRunner.h b/ETBalanceRunner.h
index afada5b..1777639 100644 (file)
--- a/ETBalanceRunner.h
+++ b/ETBalanceRunner.h
@@ -14,6 +14,9 @@ class ETBalanceRunner : public Runner
        GyroSensor& m_gyro;\r
        Nxt& m_nxt;\r
        int m_gyrooffset;\r
+       bool m_bException;\r
+       int m_msec;\r
+\r
 public:\r
        ETBalanceRunner(Motor& motorL, Motor& motorR, GyroSensor& gyro, Nxt& nxt);\r
        virtual ~ETBalanceRunner(void);\r
@@ -23,6 +26,8 @@ public:
        virtual void Stop();\r
        virtual void Reset();\r
        void SetGyroOffset(int offset);\r
+private:\r
+       void JudgeException(int l,int r);\r
 };\r
 \r
 }\r