From: kanja <hoge@hoge.hg>
Date: Thu, 12 Sep 2013 13:03:10 +0000 (+0900)
Subject: balance_controlの出力が張り付いたときのフェールセーフを実装したつもり。
X-Git-Url: http://git.osdn.net/view?p=tondenhei%2Fet2013.git;a=commitdiff_plain;h=6118ea963a83fd3abed22f2035f77fc9d10fa711

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

diff --git a/ETBalanceRunner.cpp b/ETBalanceRunner.cpp
index b227673..659d899 100644
--- a/ETBalanceRunner.cpp
+++ b/ETBalanceRunner.cpp
@@ -10,7 +10,7 @@ extern "C" {
 namespace ecrobot{
 
 ETBalanceRunner::ETBalanceRunner(Motor& motorL, Motor& motorR, GyroSensor& gyro, Nxt& nxt)
-	: m_motorL(motorL), m_motorR(motorR), m_gyro(gyro), m_nxt(nxt), m_gyrooffset(610)
+	: m_motorL(motorL), m_motorR(motorR), m_gyro(gyro), m_nxt(nxt), m_gyrooffset(610), m_bException(false), m_msec(0)
 {
 	balance_init();
 }
@@ -21,6 +21,11 @@ void ETBalanceRunner::Run(int forward, int turn)
 {
 	S8 r,l;
 	balance_control(forward,0,m_gyro.get(),m_gyrooffset,m_motorL.getCount(),m_motorR.getCount(),m_nxt.getBattMv(),&l,&r);
+	JudgeException(l,r);
+	if(IsException()){
+		Stop();
+		return;
+	}
 	l = l + turn;
 	r = r - turn;
 	if(100 < l){
@@ -35,7 +40,7 @@ void ETBalanceRunner::Run(int forward, int turn)
 }
 bool ETBalanceRunner::IsException() const
 {
-	return false;
+	return m_bException;
 }
 void ETBalanceRunner::Stop()
 {
@@ -45,10 +50,26 @@ void ETBalanceRunner::Stop()
 void ETBalanceRunner::Reset()
 {
 	balance_init();
+	m_bException = false;
+	m_msec = 0;
 }
 void ETBalanceRunner::SetGyroOffset(int offset)
 {
 	m_gyrooffset = offset;
 }
+void ETBalanceRunner::JudgeException(int l,int r)
+{
+	static const int LIMIT = 2000;	/* 2[sec] */
+	if((l == 100 && r == 100) || (l == -100 && r == -100)){	/* +100‚©-100‚É’£‚è•t‚« */
+		if(m_msec >= LIMIT){	/* ˆê’莞ŠÔ}100‚ðˆÛŽ‚µ‚½‚ç—áŠO */
+			m_bException = true;
+		}
+		else{
+			m_msec += 4;	/* +4[msec] */
+		}
+	}else{
+		m_msec = 0;
+	}
+}
 
 }
diff --git a/ETBalanceRunner.h b/ETBalanceRunner.h
index afada5b..1777639 100644
--- a/ETBalanceRunner.h
+++ b/ETBalanceRunner.h
@@ -14,6 +14,9 @@ class ETBalanceRunner : public Runner
 	GyroSensor& m_gyro;
 	Nxt& m_nxt;
 	int m_gyrooffset;
+	bool m_bException;
+	int m_msec;
+
 public:
 	ETBalanceRunner(Motor& motorL, Motor& motorR, GyroSensor& gyro, Nxt& nxt);
 	virtual ~ETBalanceRunner(void);
@@ -23,6 +26,8 @@ public:
 	virtual void Stop();
 	virtual void Reset();
 	void SetGyroOffset(int offset);
+private:
+	void JudgeException(int l,int r);
 };
 
 }