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 linePosition = 0;
int lineDifferential = 0; int lineDifferential = 0;
int lineDifferential16 = 0;
long lineIntegral = 0; long lineIntegral = 0;
// 右のモーターと左のモーターの速度差 : (右のモーター)-(左のモーター) // 右のモーターと左のモーターの速度差 : (右のモーター)-(左のモーター)
@ -158,7 +159,7 @@ void setup(){
delay(3000); delay(3000);
/* スタート */ /* スタート */
mode = MODE_STRAIGHT; mode = MODE_STRAIGHT;
MsTimer2::set(1, run); MsTimer2::set(2, run); //500Hz
MsTimer2::start(); MsTimer2::start();
} }
@ -166,7 +167,7 @@ void loop(){
sensorRead(); sensorRead();
} }
/* 車体の動作を決定する(1kHz) */ /* 車体の動作を決定する(500Hz) */
void run(){ void run(){
switch(mode){ switch(mode){
default: default:

16
hanzo4_1Utils.ino
View file

@ -78,8 +78,18 @@ int scanSensor(){
errorCount++; errorCount++;
return 127; return 127;
} }
errorCount = 0; errorCount = 0;
lineDifferential = pos - linePosition;
static int forDiv16;
if(forDiv16<16){
lineDifferential += pos - linePosition;
forDiv16++;
}else{
lineDifferential16 = lineDifferential;
lineDifferential = 0;
forDiv16 = 0;
}
linePosition = pos; linePosition = pos;
lineIntegral += pos; lineIntegral += pos;
return pos; return pos;
@ -87,12 +97,12 @@ int scanSensor(){
// 右のモーターと左のモーターの速度差 : (右のモーター)-(左のモーター) // 右のモーターと左のモーターの速度差 : (右のモーター)-(左のモーター)
int speedPIDControl(){ int speedPIDControl(){
return speedKP*linePosition + speedKI*lineIntegral + speedKD*lineDifferential; return speedKP*linePosition + speedKI*lineIntegral + speedKD*lineDifferential16/16;
} }
// ステアリングの角度(度) // ステアリングの角度(度)
int steeringPIDControl(){ int steeringPIDControl(){
return steeringKP*linePosition + steeringKI*lineIntegral + steeringKD*lineDifferential; return steeringKP*linePosition + steeringKI*lineIntegral + steeringKD*lineDifferential16/16;
} }
/* エンコーダーの割り込み処理 */ /* エンコーダーの割り込み処理 */