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

細かいパラメータの調整

Browse source
This commit is contained in:
Nebel 2014-03-21 01:45:21 +09:00
parent 60174616df
commit 3b56ac95e7
2 changed files with 109 additions and 122 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

151
hanzo4_1.ino
View file

@ -34,41 +34,36 @@ struct target_t{
unsigned long lapDistance = 28500; //ロータリーエンコーダーのカウント数/周 unsigned long lapDistance = 28500; //ロータリーエンコーダーのカウント数/周
unsigned long turnDistance = 4650; //カーブ中でのカウント数 unsigned long turnDistance = 4650; //カーブ中でのカウント数
struct target_t targetA = {{lapDistance/2, lapDistance*0.6 }, 1500, { 29, 0}}; struct target_t targetA = {{lapDistance/2, lapDistance*0.6 }, 900, { 29, 0}};
struct target_t targetB = {{lapDistance/2, lapDistance*0.75}, 1500, {-32, 0}}; struct target_t targetB = {{lapDistance/2, lapDistance*0.75}, 900, {-32, 0}};
struct target_t targetC = {{lapDistance*0.6, lapDistance*0.85}, 1500, { 34, 0}}; struct target_t targetC = {{lapDistance*0.6, lapDistance*0.85}, 900, { 34, 0}};
struct target_t targetD = {{lapDistance*0.75, lapDistance*0.85}, 1500, {-42, 0}}; struct target_t targetD = {{lapDistance*0.75, lapDistance*0.85}, 900, {-42, 0}};
struct target_t targetE = {{0, lapDistance/8 }, 800, { 40, 48}}; struct target_t targetE = {{1500, 2400 }, 800, { 40, 48}};
struct target_t targetF = {{lapDistance/8, lapDistance/4 }, 1000, { 61, 69}}; struct target_t targetF = {{lapDistance/8, lapDistance/4 }, 800, { 61, 60}};
int laps = 1; //最大周回数 int laps = 3; //最大周回数
bool silent = true; //サイレントモード(true:ブザーを鳴らさない) bool silent = false; //サイレントモード(true:ブザーを鳴らさない)
int steeringAngleMin = -30; //度 int steeringAngleMin = -30; //度
int steeringAngleMax = 30; //度 int steeringAngleMax = 10; //度
int speedMax = 100; //最大速度 int deltaSpeedMax = 500; //最大速度差
int speedMax = 255; //最大速度
float steeringKP = 3.8; int cycle = 1; //動作周波数(ms)
float steeringKI = 0.0;
float steeringKD = 0.0;
float speedKP = 50.0; int errorCountThreshold = 500; //最大読み取りエラー回数(これを超えると停止する)
float speedKI = 0.0;
float speedKD = 0.0;
int errorCountThreshold = 5000; //最大読み取りエラー回数(これを超えると停止する)
/**************************************/ /**************************************/
/* ピンの設定 */ /* ピンの設定 */
// Servo : SC-1267SG // Servo : SC-1267SG
unsigned int steeringServoCenter = 1810; //us unsigned int steeringServoCenter = 1815; //us
unsigned int steeringServoMin = (steeringServoCenter-780); //us unsigned int steeringServoMin = (steeringServoCenter-780); //us
unsigned int steeringServoMax = (steeringServoCenter+780); //us unsigned int steeringServoMax = (steeringServoCenter+780); //us
// Servo : SC-0352 // Servo : SC-0352
unsigned int lanceServoCenter = 1530; //us unsigned int lanceServoCenter = 1550; //us
unsigned int lanceServoMin = (lanceServoCenter-780); //us unsigned int lanceServoMin = (lanceServoCenter-780); //us
unsigned int lanceServoMax = (lanceServoCenter+780); //us unsigned int lanceServoMax = (lanceServoCenter+780); //us
@ -100,12 +95,9 @@ Motor motorL;
//ラインが1、地面が0、LSBが左端、MSBが右端 //ラインが1、地面が0、LSBが左端、MSBが右端
unsigned char sensor = 0x00; unsigned char sensor = 0x00;
int linePosition = 0;
int lineDifferential = 0;
int lineDifferential16 = 0;
long lineIntegral = 0;
// 右のモーターと左のモーターの速度差 : (右のモーター)-(左のモーター) // 右のモーターと左のモーターの速度差 : (右のモーター)-(左のモーター)
int deltaSpeed = 0;
// 駆動用モーターの速度
int speed = 0; int speed = 0;
// ステアリングの角度 // ステアリングの角度
@ -117,8 +109,8 @@ volatile unsigned long distance = 0;
unsigned long modeChangedDistance = 0; unsigned long modeChangedDistance = 0;
/**************************************/ /**************************************/
/* 動作モード */
/* 動作モード */
enum mode_t { enum mode_t {
MODE_STOP, //停止 MODE_STOP, //停止
MODE_STRAIGHT, //まっすぐ進む MODE_STRAIGHT, //まっすぐ進む
@ -134,6 +126,73 @@ enum mode_t {
/**************************************/ /**************************************/
/* センサーを読んで、車体の動作を決定する */
void trace(){
switch(sensor&lineSensorMask){
default:
errorCount++;
return;
case 0b01000000:
speed = speedMax*0.3;
deltaSpeed = -150;
angle = -18;
break;
case 0b01100000:
speed = speedMax*0.5;
deltaSpeed = -100;
angle = -16;
break;
case 0b00100000:
speed = speedMax*0.6;
deltaSpeed = -50;
angle = -14;
break;
case 0b00110000:
speed = speedMax;
deltaSpeed = -25;
angle = -10;
break;
case 0b00010000:
speed = speedMax;
deltaSpeed = 0;
angle = -1;
break;
case 0b00011000:
speed = speedMax;
deltaSpeed = 0;
angle = 0;
break;
case 0b00001000:
speed = speedMax;
deltaSpeed = 0;
angle = 1;
break;
case 0b00001100:
speed = speedMax;
deltaSpeed = 0;
angle = 2;
break;
case 0b00000100:
speed = speedMax;
deltaSpeed = 3;
angle = 3;
break;
case 0b00000110:
speed = speedMax;
deltaSpeed = 4;
angle = 4;
break;
case 0b00000010:
speed = 0;
deltaSpeed = 5;
angle = 5;
break;
}
errorCount = 0;
}
/**************************************/
void setup(){ void setup(){
//サーボ //サーボ
steeringServo.attach(steeringServoPin, steeringServoMin, steeringServoMax); steeringServo.attach(steeringServoPin, steeringServoMin, steeringServoMax);
@ -151,24 +210,26 @@ void setup(){
pinMode(rotPin, INPUT); pinMode(rotPin, INPUT);
attachInterrupt(0, rot, RISING); attachInterrupt(0, rot, RISING);
//ブザー //ブザー
pinMode(buzzerPin, OUTPUT); startBeep();
if(!silent) startBeep();
/* 3秒停止 */ /* 3秒停止 */
motorL.mode(STOP); motorL.mode(STOP);
motorR.mode(STOP); motorR.mode(STOP);
delay(3000); delay(3000);
/* スタート */ /* スタート */
mode = MODE_STRAIGHT; mode = MODE_STRAIGHT;
MsTimer2::set(2, run); //500Hz MsTimer2::set(cycle, run);
MsTimer2::start(); MsTimer2::start();
} }
void loop(){ void loop(){
sensorRead(); scanSensor();
} }
/* 車体の動作を決定する(500Hz) */ /* 車体の動作を決定する */
void run(){ void run(){
if(distance > lapDistance*laps || errorCount > errorCountThreshold){
mode = MODE_STOP;
}
switch(mode){ switch(mode){
default: default:
case MODE_STOP: //停止 case MODE_STOP: //停止
@ -176,7 +237,6 @@ void run(){
steeringServo.detach(); steeringServo.detach();
motorL.mode(STOP); motorL.mode(STOP);
motorR.mode(STOP); motorR.mode(STOP);
if(!silent) stopBeep();
while(1){ while(1){
; ;
}; };
@ -244,33 +304,24 @@ void run(){
mode = MODE_STRAIGHT; mode = MODE_STRAIGHT;
break; break;
} }
}
/* 駆動モーター用 */
/* センサーを読んで、ライントレースする */ deltaSpeed = constrain(deltaSpeed, -deltaSpeedMax, deltaSpeedMax);
void trace(){ if(deltaSpeed<0){
scanSensor(); motorL.speed(speed + deltaSpeed);
motorR.speed(speed);
speed = speedPIDControl();
speed = constrain(speed, -speedMax, speedMax);
if(speed<0){
motorL.speed(speedMax + speed);
motorR.speed(speedMax);
}else{ }else{
motorL.speed(speedMax); motorL.speed(speed);
motorR.speed(speedMax - speed); motorR.speed(speed - deltaSpeed);
} }
angle = steeringPIDControl(); /* ステアリングサーボ用 */
angle = constrain(angle, steeringAngleMin, steeringAngleMax); angle = constrain(angle, steeringAngleMin, steeringAngleMax);
steering(angle); steering(angle);
} }
/* モードを決定する */ /* モードを決定する */
void modeSet(){ void modeSet(){
if(distance > lapDistance*laps || errorCount > errorCountThreshold){
mode = MODE_STOP;
return;
}
unsigned long way = distance%lapDistance; unsigned long way = distance%lapDistance;
switch(sensor&markerSensorMask){ switch(sensor&markerSensorMask){
default: default:

80
hanzo4_1Utils.ino
View file

@ -5,8 +5,9 @@
[]---{}---[] []---{}---[]
/ .\ / .\
*/ */
void steering(int a){ int steering(int a){
steeringServo.write(90 + a); steeringServo.write(90 + constrain(a, steeringAngleMin, steeringAngleMax));
return (steeringServo.read() - 90);
} }
/* ランスを中央からa[度]だけ動かす /* ランスを中央からa[度]だけ動かす
@ -15,8 +16,9 @@ void steering(int a){
/ | \ / | \
[]+--{}--+[] []+--{}--+[]
*/ */
void lance(int a){ int lance(int a){
lanceServo.write(90 - a); lanceServo.write(90 - a);
return (90 - a);
} }
/* センサーのアレイの初期化 */ /* センサーのアレイの初期化 */
@ -27,7 +29,7 @@ void sensorInit(){
} }
/* センサーアレイの状態を見る */ /* センサーアレイの状態を見る */
unsigned char sensorRead(){ unsigned char scanSensor(){
unsigned char b; unsigned char b;
//黒が1、白が0 //黒が1、白が0
for(int i=0; i<numSensors; i++){ for(int i=0; i<numSensors; i++){
@ -37,74 +39,6 @@ unsigned char sensorRead(){
return sensor = b; return sensor = b;
} }
/* センサーを読んでラインの位置を返す */
int scanSensor(){
int pos = 0;
switch(sensor&lineSensorMask){
case 0b01000000:
pos = -5;
break;
case 0b01100000:
pos = -4;
break;
case 0b00100000:
pos = -3;
break;
case 0b00110000:
pos = -2;
break;
case 0b00010000:
pos = -1;
break;
case 0b00011000:
pos = 0;
break;
case 0b00001000:
pos = 1;
break;
case 0b00001100:
pos = 2;
break;
case 0b00000100:
pos = 3;
break;
case 0b00000110:
pos = 4;
break;
case 0b00000010:
pos = 5;
break;
default:
errorCount++;
return 127;
}
errorCount = 0;
static int forDiv16;
if(forDiv16<16){
lineDifferential += pos - linePosition;
forDiv16++;
}else{
lineDifferential16 = lineDifferential;
lineDifferential = 0;
forDiv16 = 0;
}
linePosition = pos;
lineIntegral += pos;
return pos;
}
// 右のモーターと左のモーターの速度差 : (右のモーター)-(左のモーター)
int speedPIDControl(){
return speedKP*linePosition + speedKI*lineIntegral + speedKD*lineDifferential16/16;
}
// ステアリングの角度(度)
int steeringPIDControl(){
return steeringKP*linePosition + steeringKI*lineIntegral + steeringKD*lineDifferential16/16;
}
/* エンコーダーの割り込み処理 */ /* エンコーダーの割り込み処理 */
void rot(){ void rot(){
distance++; distance++;
@ -112,6 +46,7 @@ void rot(){
/* 起動音 */ /* 起動音 */
void startBeep(){ void startBeep(){
pinMode(buzzerPin, OUTPUT);
tone(buzzerPin,2000); tone(buzzerPin,2000);
delay(100); delay(100);
tone(buzzerPin,1000); tone(buzzerPin,1000);
@ -121,6 +56,7 @@ void startBeep(){
/* 終了音 */ /* 終了音 */
void stopBeep(){ void stopBeep(){
pinMode(buzzerPin, OUTPUT);
tone(buzzerPin, 2000); tone(buzzerPin, 2000);
delay(2000); delay(2000);
noTone(buzzerPin); noTone(buzzerPin);