From 60174616df5a9e1db9c49b088b759fd8b904514b Mon Sep 17 00:00:00 2001
From: kou029w <kou029w@gmail.com>
Date: Wed, 19 Mar 2014 11:53:15 +0900
Subject: [PATCH] =?UTF-8?q?=E3=82=B5=E3=83=B3=E3=83=97=E3=83=AA=E3=83=B3?=
 =?UTF-8?q?=E3=82=B0=E5=91=A8=E6=9C=9F=E3=81=8C=E7=9F=AD=E3=81=99=E3=81=8E?=
 =?UTF-8?q?=E3=82=8B=E3=82=88=E3=81=86=E3=81=AB=E6=84=9F=E3=81=98=E3=81=9F?=
 =?UTF-8?q?=E3=81=AE=E3=81=A71kHz=E3=81=8B=E3=82=89500kHz=E3=81=AB?=
 =?UTF-8?q?=E5=A4=89=E6=9B=B4?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 hanzo4_1.ino      |  5 +++--
 hanzo4_1Utils.ino | 16 +++++++++++++---
 2 files changed, 16 insertions(+), 5 deletions(-)

diff --git a/hanzo4_1.ino b/hanzo4_1.ino
index ecd8020..3c6804d 100644
--- a/hanzo4_1.ino
+++ b/hanzo4_1.ino
@@ -102,6 +102,7 @@ unsigned char sensor = 0x00;
 
 int linePosition = 0;
 int lineDifferential = 0;
+int lineDifferential16 = 0;
 long lineIntegral = 0;
 
 // 右のモーターと左のモーターの速度差 : (右のモーター)-(左のモーター)
@@ -158,7 +159,7 @@ void setup(){
   delay(3000);
   /* スタート */
   mode = MODE_STRAIGHT;
-  MsTimer2::set(1, run);
+  MsTimer2::set(2, run);  //500Hz
   MsTimer2::start();
 }
 
@@ -166,7 +167,7 @@ void loop(){
   sensorRead();
 }
 
-/* 車体の動作を決定する(1kHz) */
+/* 車体の動作を決定する(500Hz) */
 void run(){
   switch(mode){
   default:
diff --git a/hanzo4_1Utils.ino b/hanzo4_1Utils.ino
index cbd7b4e..7fce024 100644
--- a/hanzo4_1Utils.ino
+++ b/hanzo4_1Utils.ino
@@ -78,8 +78,18 @@ int scanSensor(){
     errorCount++;
     return 127;
   }
+  
   errorCount = 0;
-  lineDifferential = pos - linePosition;
+  
+  static int forDiv16;
+  if(forDiv16<16){
+    lineDifferential += pos - linePosition;
+    forDiv16++;
+  }else{
+    lineDifferential16 = lineDifferential;
+    lineDifferential = 0;
+    forDiv16 = 0;
+  }
   linePosition = pos;
   lineIntegral += pos;
   return pos;
@@ -87,12 +97,12 @@ int scanSensor(){
 
 // 右のモーターと左のモーターの速度差 : (右のモーター)-(左のモーター)
 int speedPIDControl(){
-  return speedKP*linePosition + speedKI*lineIntegral + speedKD*lineDifferential;
+  return speedKP*linePosition + speedKI*lineIntegral + speedKD*lineDifferential16/16;
 }
 
 // ステアリングの角度(度)
 int steeringPIDControl(){
-  return steeringKP*linePosition + steeringKI*lineIntegral + steeringKD*lineDifferential;
+  return steeringKP*linePosition + steeringKI*lineIntegral + steeringKD*lineDifferential16/16;
 }
 
 /* エンコーダーの割り込み処理 */