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; } /* エンコーダーの割り込み処理 */