From 3b56ac95e7273dcf681263b39eea3430a584721a Mon Sep 17 00:00:00 2001 From: kou029w Date: Fri, 21 Mar 2014 01:45:21 +0900 Subject: [PATCH] =?UTF-8?q?=E7=B4=B0=E3=81=8B=E3=81=84=E3=83=91=E3=83=A9?= =?UTF-8?q?=E3=83=A1=E3=83=BC=E3=82=BF=E3=81=AE=E8=AA=BF=E6=95=B4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- hanzo4_1.ino | 151 +++++++++++++++++++++++++++++++--------------- hanzo4_1Utils.ino | 80 +++--------------------- 2 files changed, 109 insertions(+), 122 deletions(-) diff --git a/hanzo4_1.ino b/hanzo4_1.ino index 3c6804d..530bde9 100644 --- a/hanzo4_1.ino +++ b/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: diff --git a/hanzo4_1Utils.ino b/hanzo4_1Utils.ino index 7fce024..522dd7c 100644 --- a/hanzo4_1Utils.ino +++ b/hanzo4_1Utils.ino @@ -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