nebel/hanzo
1
0
Fork 0
Code Issues Pull requests Projects Releases Wiki Activity

サンプリング周期が短すぎるように感じたので1kHzから500kHzに変更

Browse source
This commit is contained in:
Nebel 2014-03-19 11:53:15 +09:00
parent b7e98fc2f8
commit 60174616df
2 changed files with 16 additions and 5 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

5
hanzo4_1.ino
View file

@ -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:

16
hanzo4_1Utils.ino
View file

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