細かいパラメータの調整
This commit is contained in:
parent
60174616df
commit
3b56ac95e7
2 changed files with 109 additions and 122 deletions
149
hanzo4_1.ino
149
hanzo4_1.ino
|
|
@ -34,41 +34,36 @@ struct target_t{
|
|||
unsigned long lapDistance = 28500; //ロータリーエンコーダーのカウント数/周
|
||||
unsigned long turnDistance = 4650; //カーブ中でのカウント数
|
||||
|
||||
struct target_t targetA = {{lapDistance/2, lapDistance*0.6 }, 1500, { 29, 0}};
|
||||
struct target_t targetB = {{lapDistance/2, lapDistance*0.75}, 1500, {-32, 0}};
|
||||
struct target_t targetC = {{lapDistance*0.6, lapDistance*0.85}, 1500, { 34, 0}};
|
||||
struct target_t targetD = {{lapDistance*0.75, lapDistance*0.85}, 1500, {-42, 0}};
|
||||
struct target_t targetE = {{0, lapDistance/8 }, 800, { 40, 48}};
|
||||
struct target_t targetF = {{lapDistance/8, lapDistance/4 }, 1000, { 61, 69}};
|
||||
struct target_t targetA = {{lapDistance/2, lapDistance*0.6 }, 900, { 29, 0}};
|
||||
struct target_t targetB = {{lapDistance/2, lapDistance*0.75}, 900, {-32, 0}};
|
||||
struct target_t targetC = {{lapDistance*0.6, lapDistance*0.85}, 900, { 34, 0}};
|
||||
struct target_t targetD = {{lapDistance*0.75, lapDistance*0.85}, 900, {-42, 0}};
|
||||
struct target_t targetE = {{1500, 2400 }, 800, { 40, 48}};
|
||||
struct target_t targetF = {{lapDistance/8, lapDistance/4 }, 800, { 61, 60}};
|
||||
|
||||
int laps = 1; //最大周回数
|
||||
bool silent = true; //サイレントモード(true:ブザーを鳴らさない)
|
||||
int laps = 3; //最大周回数
|
||||
bool silent = false; //サイレントモード(true:ブザーを鳴らさない)
|
||||
|
||||
int steeringAngleMin = -30; //度
|
||||
int steeringAngleMax = 30; //度
|
||||
int steeringAngleMax = 10; //度
|
||||
|
||||
int speedMax = 100; //最大速度
|
||||
int deltaSpeedMax = 500; //最大速度差
|
||||
int speedMax = 255; //最大速度
|
||||
|
||||
float steeringKP = 3.8;
|
||||
float steeringKI = 0.0;
|
||||
float steeringKD = 0.0;
|
||||
int cycle = 1; //動作周波数(ms)
|
||||
|
||||
float speedKP = 50.0;
|
||||
float speedKI = 0.0;
|
||||
float speedKD = 0.0;
|
||||
|
||||
int errorCountThreshold = 5000; //最大読み取りエラー回数(これを超えると停止する)
|
||||
int errorCountThreshold = 500; //最大読み取りエラー回数(これを超えると停止する)
|
||||
|
||||
/**************************************/
|
||||
/* ピンの設定 */
|
||||
|
||||
// Servo : SC-1267SG
|
||||
unsigned int steeringServoCenter = 1810; //us
|
||||
unsigned int steeringServoCenter = 1815; //us
|
||||
unsigned int steeringServoMin = (steeringServoCenter-780); //us
|
||||
unsigned int steeringServoMax = (steeringServoCenter+780); //us
|
||||
|
||||
// Servo : SC-0352
|
||||
unsigned int lanceServoCenter = 1530; //us
|
||||
unsigned int lanceServoCenter = 1550; //us
|
||||
unsigned int lanceServoMin = (lanceServoCenter-780); //us
|
||||
unsigned int lanceServoMax = (lanceServoCenter+780); //us
|
||||
|
||||
|
|
@ -100,12 +95,9 @@ Motor motorL;
|
|||
//ラインが1、地面が0、LSBが左端、MSBが右端
|
||||
unsigned char sensor = 0x00;
|
||||
|
||||
int linePosition = 0;
|
||||
int lineDifferential = 0;
|
||||
int lineDifferential16 = 0;
|
||||
long lineIntegral = 0;
|
||||
|
||||
// 右のモーターと左のモーターの速度差 : (右のモーター)-(左のモーター)
|
||||
int deltaSpeed = 0;
|
||||
// 駆動用モーターの速度
|
||||
int speed = 0;
|
||||
|
||||
// ステアリングの角度
|
||||
|
|
@ -117,8 +109,8 @@ volatile unsigned long distance = 0;
|
|||
unsigned long modeChangedDistance = 0;
|
||||
|
||||
/**************************************/
|
||||
/* 動作モード */
|
||||
|
||||
/* 動作モード */
|
||||
enum mode_t {
|
||||
MODE_STOP, //停止
|
||||
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(){
|
||||
//サーボ
|
||||
steeringServo.attach(steeringServoPin, steeringServoMin, steeringServoMax);
|
||||
|
|
@ -151,24 +210,26 @@ void setup(){
|
|||
pinMode(rotPin, INPUT);
|
||||
attachInterrupt(0, rot, RISING);
|
||||
//ブザー
|
||||
pinMode(buzzerPin, OUTPUT);
|
||||
if(!silent) startBeep();
|
||||
startBeep();
|
||||
/* 3秒停止 */
|
||||
motorL.mode(STOP);
|
||||
motorR.mode(STOP);
|
||||
delay(3000);
|
||||
/* スタート */
|
||||
mode = MODE_STRAIGHT;
|
||||
MsTimer2::set(2, run); //500Hz
|
||||
MsTimer2::set(cycle, run);
|
||||
MsTimer2::start();
|
||||
}
|
||||
|
||||
void loop(){
|
||||
sensorRead();
|
||||
scanSensor();
|
||||
}
|
||||
|
||||
/* 車体の動作を決定する(500Hz) */
|
||||
/* 車体の動作を決定する */
|
||||
void run(){
|
||||
if(distance > lapDistance*laps || errorCount > errorCountThreshold){
|
||||
mode = MODE_STOP;
|
||||
}
|
||||
switch(mode){
|
||||
default:
|
||||
case MODE_STOP: //停止
|
||||
|
|
@ -176,7 +237,6 @@ void run(){
|
|||
steeringServo.detach();
|
||||
motorL.mode(STOP);
|
||||
motorR.mode(STOP);
|
||||
if(!silent) stopBeep();
|
||||
while(1){
|
||||
;
|
||||
};
|
||||
|
|
@ -244,33 +304,24 @@ void run(){
|
|||
mode = MODE_STRAIGHT;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
/* センサーを読んで、ライントレースする */
|
||||
void trace(){
|
||||
scanSensor();
|
||||
|
||||
speed = speedPIDControl();
|
||||
speed = constrain(speed, -speedMax, speedMax);
|
||||
if(speed<0){
|
||||
motorL.speed(speedMax + speed);
|
||||
motorR.speed(speedMax);
|
||||
/* 駆動モーター用 */
|
||||
deltaSpeed = constrain(deltaSpeed, -deltaSpeedMax, deltaSpeedMax);
|
||||
if(deltaSpeed<0){
|
||||
motorL.speed(speed + deltaSpeed);
|
||||
motorR.speed(speed);
|
||||
}else{
|
||||
motorL.speed(speedMax);
|
||||
motorR.speed(speedMax - speed);
|
||||
motorL.speed(speed);
|
||||
motorR.speed(speed - deltaSpeed);
|
||||
}
|
||||
|
||||
angle = steeringPIDControl();
|
||||
/* ステアリングサーボ用 */
|
||||
angle = constrain(angle, steeringAngleMin, steeringAngleMax);
|
||||
steering(angle);
|
||||
}
|
||||
|
||||
/* モードを決定する */
|
||||
void modeSet(){
|
||||
if(distance > lapDistance*laps || errorCount > errorCountThreshold){
|
||||
mode = MODE_STOP;
|
||||
return;
|
||||
}
|
||||
unsigned long way = distance%lapDistance;
|
||||
switch(sensor&markerSensorMask){
|
||||
default:
|
||||
|
|
|
|||
|
|
@ -5,8 +5,9 @@
|
|||
[]---{}---[]
|
||||
/ .\
|
||||
*/
|
||||
void steering(int a){
|
||||
steeringServo.write(90 + a);
|
||||
int steering(int a){
|
||||
steeringServo.write(90 + constrain(a, steeringAngleMin, steeringAngleMax));
|
||||
return (steeringServo.read() - 90);
|
||||
}
|
||||
|
||||
/* ランスを中央からa[度]だけ動かす
|
||||
|
|
@ -15,8 +16,9 @@ void steering(int a){
|
|||
/ | \
|
||||
[]+--{}--+[]
|
||||
*/
|
||||
void lance(int a){
|
||||
int lance(int a){
|
||||
lanceServo.write(90 - a);
|
||||
return (90 - a);
|
||||
}
|
||||
|
||||
/* センサーのアレイの初期化 */
|
||||
|
|
@ -27,7 +29,7 @@ void sensorInit(){
|
|||
}
|
||||
|
||||
/* センサーアレイの状態を見る */
|
||||
unsigned char sensorRead(){
|
||||
unsigned char scanSensor(){
|
||||
unsigned char b;
|
||||
//黒が1、白が0
|
||||
for(int i=0; i<numSensors; i++){
|
||||
|
|
@ -37,74 +39,6 @@ unsigned char sensorRead(){
|
|||
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(){
|
||||
distance++;
|
||||
|
|
@ -112,6 +46,7 @@ void rot(){
|
|||
|
||||
/* 起動音 */
|
||||
void startBeep(){
|
||||
pinMode(buzzerPin, OUTPUT);
|
||||
tone(buzzerPin,2000);
|
||||
delay(100);
|
||||
tone(buzzerPin,1000);
|
||||
|
|
@ -121,6 +56,7 @@ void startBeep(){
|
|||
|
||||
/* 終了音 */
|
||||
void stopBeep(){
|
||||
pinMode(buzzerPin, OUTPUT);
|
||||
tone(buzzerPin, 2000);
|
||||
delay(2000);
|
||||
noTone(buzzerPin);
|
||||
|
|
|
|||
Loading…
Add table
Reference in a new issue