nebel/2014-git-work
1
0
Fork 0
Code Issues Pull requests Projects Releases Wiki Activity

コメントを追加

Browse source
This commit is contained in:
Nebel 2012-10-09 20:03:53 +09:00
parent 08ab4cd847
commit 440326dbe4
1 changed files with 94 additions and 81 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

175
arduino/hanzo2_1/hanzo2_1.ino
View file

@ -16,15 +16,13 @@
#include <Servo.h> #include <Servo.h>
#include "Motor.h" #include "Motor.h"
//#define LINE_BLACK //白地・黒ライン有効 const unsigned long LAP_COUNT = 848; //カウント/周
const unsigned long TARGET_NONE_COUNT = LAP_COUNT*90/848; //カーブを出るカウント
const unsigned long LAP_COUNT = 848;
const unsigned long TARGET_NONE_COUNT = LAP_COUNT*90/848;
const int LANCE_ANGLE_PARALLEL1 = 90 - 53; //度 const int LANCE_ANGLE_PARALLEL1 = 90 - 53; //度
const int LANCE_ANGLE_PARALLEL2 = 90 - 43; const int LANCE_ANGLE_PARALLEL2 = 90 - 43; //度
const int LANCE_ANGLE_LEFT_VERTICAL = 90 - 120; const int LANCE_ANGLE_LEFT_VERTICAL = 90 - 120; //度
const int LANCE_ANGLE_RIGHT_VERTICAL = 90 - 57; const int LANCE_ANGLE_RIGHT_VERTICAL = 90 - 57; //度
const byte SPEED_DEFAULT = 255; const byte SPEED_DEFAULT = 255;
@ -68,8 +66,8 @@ enum {
MODE_TARGET_CYLINDER //円筒標的 MODE_TARGET_CYLINDER //円筒標的
}; };
const byte MASK_MODE_TRACE = 0b01111110; const byte MASK_MODE_TRACE = 0b01111110;
const byte MASK_CHECK_MARKER = 0b10000001; const byte MASK_CHECK_MARKER = 0b10000001;
/**************************************/ /**************************************/
@ -84,55 +82,7 @@ unsigned long modeChangeCounter;
/**************************************/ /**************************************/
void traceLeft(byte sensor){ /* センサーを読んで、車体の動作を決定する */
switch(sensor&MASK_MODE_TRACE){
case 0b01000000:
handle(-5*HANDLE_KP);
motorMode(GO, GO, 0, SPEED_DEFAULT);
break;
case 0b01100000:
handle(-4*HANDLE_KP);
motorMode(GO, GO, 0, SPEED_DEFAULT);
break;
case 0b00100000:
handle(-3*HANDLE_KP);
motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
break;
case 0b00110000:
handle(-2*HANDLE_KP);
motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
break;
case 0b00010000:
handle(-2);
motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
break;
case 0b00011000:
handle(0);
motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
break;
case 0b00001000:
handle(2);
motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
break;
case 0b00001100:
handle(2*HANDLE_KP);
motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
break;
case 0b00000100:
handle(3*HANDLE_KP);
motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
break;
case 0b00000110:
handle(4*HANDLE_KP);
motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
break;
case 0b00000010:
handle(5*HANDLE_KP);
motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
break;
}
}
void trace(byte sensor){ void trace(byte sensor){
switch(sensor&MASK_MODE_TRACE){ switch(sensor&MASK_MODE_TRACE){
case 0b01000000: case 0b01000000:
@ -182,23 +132,83 @@ void trace(byte sensor){
} }
} }
/* MODE_LEFT時の再加速用 */
void traceLeft(byte sensor){
switch(sensor&MASK_MODE_TRACE){
case 0b01000000:
handle(-5*HANDLE_KP);
motorMode(GO, GO, 0, SPEED_DEFAULT);
break;
case 0b01100000:
handle(-4*HANDLE_KP);
motorMode(GO, GO, 0, SPEED_DEFAULT);
break;
case 0b00100000:
handle(-3*HANDLE_KP);
motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
break;
case 0b00110000:
handle(-2*HANDLE_KP);
motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
break;
case 0b00010000:
handle(-2);
motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
break;
case 0b00011000:
handle(0);
motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
break;
case 0b00001000:
handle(2);
motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
break;
case 0b00001100:
handle(2*HANDLE_KP);
motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
break;
case 0b00000100:
handle(3*HANDLE_KP);
motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
break;
case 0b00000110:
handle(4*HANDLE_KP);
motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
break;
case 0b00000010:
handle(5*HANDLE_KP);
motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
break;
}
}
/* モーターの動作を決定する */
void motorMode(byte leftMode, byte rightMode, byte leftSpeed, byte rightSpeed){ void motorMode(byte leftMode, byte rightMode, byte leftSpeed, byte rightSpeed){
motorL.mode( leftMode, leftSpeed); motorL.mode( leftMode, leftSpeed);
motorR.mode(rightMode, rightSpeed); motorR.mode(rightMode, rightSpeed);
} }
/* ステアリングを中央からdegree[度]だけ動かす
<- - 0 + ->
| |
| |
[]---{}---[]
/ .\
*/
void handle(int degree){ void handle(int degree){
servoHandle.write(HANDLE_DEGREE_DEFAULT + degree); servoHandle.write(HANDLE_DEGREE_DEFAULT + degree);
} }
/* ランスを中央からdegree[度]だけ動かす
<- - 0 + ->
/ .\
/ | \
[]+--{}--+[]
*/
void lance(int degree){ void lance(int degree){
servoLance.write(LANCE_DEGREE_DEFAULT - degree); servoLance.write(LANCE_DEGREE_DEFAULT - degree);
} }
int lanceRead(){
return LANCE_DEGREE_DEFAULT - servoLance.read();
}
/* ロータリーエンコーダーの変化を見る /* ロータリーエンコーダーの変化を見る
todo:外部割り込み使ってみたい、、、 todo:外部割り込み使ってみたい、、、
*/ */
@ -211,6 +221,7 @@ void count(){
} }
} }
/* マーカーを読んで、モードを返す */
int checkMarker(byte sensor){ int checkMarker(byte sensor){
int mode = MODE_TRACE; int mode = MODE_TRACE;
switch(sensor & MASK_CHECK_MARKER){ switch(sensor & MASK_CHECK_MARKER){
@ -220,11 +231,11 @@ int checkMarker(byte sensor){
break; break;
case 0b00000001: case 0b00000001:
modeChangeCounter = counter; modeChangeCounter = counter;
if(counter%LAP_COUNT > LAP_COUNT/2){ if(counter%LAP_COUNT > LAP_COUNT/2){ //後半は垂直標的をねらいたい
mode = MODE_TARGET_RIGHT_VERTICAL; mode = MODE_TARGET_RIGHT_VERTICAL;
}else if(counter%LAP_COUNT < LAP_COUNT/7){ }else if(counter%LAP_COUNT < LAP_COUNT/7){ //まずは、E的をねらいたい
mode = MODE_TARGET_PARALLEL1; mode = MODE_TARGET_PARALLEL1;
}else if(counter%LAP_COUNT < LAP_COUNT/3){ }else if(counter%LAP_COUNT < LAP_COUNT/3){ //次に、F的をねらいたい
mode = MODE_TARGET_PARALLEL2; mode = MODE_TARGET_PARALLEL2;
}else{ }else{
mode = MODE_TARGET_RIGHT_VERTICAL; mode = MODE_TARGET_RIGHT_VERTICAL;
@ -238,6 +249,7 @@ int checkMarker(byte sensor){
return mode; return mode;
} }
/* 前方センサーの初期化 */
void sensorInit(){ void sensorInit(){
pinMode(PIN_SENSOR_0, INPUT); pinMode(PIN_SENSOR_0, INPUT);
pinMode(PIN_SENSOR_1, INPUT); pinMode(PIN_SENSOR_1, INPUT);
@ -249,6 +261,7 @@ void sensorInit(){
pinMode(PIN_SENSOR_7, INPUT); pinMode(PIN_SENSOR_7, INPUT);
} }
/* センサーの状態を返す */
byte sensorRead(){ byte sensorRead(){
byte sensor = 0; //黒が1、白が0 byte sensor = 0; //黒が1、白が0
if(digitalRead(PIN_SENSOR_0)) sensor += 0b00000001; if(digitalRead(PIN_SENSOR_0)) sensor += 0b00000001;
@ -308,7 +321,7 @@ void loop(){
case MODE_TARGET_PARALLEL1: //平行標的1 case MODE_TARGET_PARALLEL1: //平行標的1
trace(sensor); trace(sensor);
lance(LANCE_ANGLE_PARALLEL1); lance(LANCE_ANGLE_PARALLEL1);
if(counter - modeChangeCounter > 24){ if(counter - modeChangeCounter > 24){ //マーカーから少し進んで、叩く
lance(LANCE_ANGLE_PARALLEL1+8); lance(LANCE_ANGLE_PARALLEL1+8);
mode = MODE_TRACE; mode = MODE_TRACE;
} }
@ -316,7 +329,7 @@ void loop(){
case MODE_TARGET_PARALLEL2: //平行標的2 case MODE_TARGET_PARALLEL2: //平行標的2
trace(sensor); trace(sensor);
lance(LANCE_ANGLE_PARALLEL2); lance(LANCE_ANGLE_PARALLEL2);
if(counter - modeChangeCounter > 26){ if(counter - modeChangeCounter > 26){ //マーカーから少し進んで、叩く
lance(LANCE_ANGLE_PARALLEL2+8); lance(LANCE_ANGLE_PARALLEL2+8);
mode = MODE_TRACE; mode = MODE_TRACE;
} }
@ -324,14 +337,14 @@ void loop(){
case MODE_TRACE: case MODE_TRACE:
trace(sensor); trace(sensor);
mode = checkMarker(sensor); mode = checkMarker(sensor);
if(counter > LAP_COUNT*10){ if(counter > LAP_COUNT*10){ //10周したら停止
mode = MODE_STOP; mode = MODE_STOP;
} }
break; break;
case MODE_LEFT: //左カーブ case MODE_LEFT: //左カーブ
if(counter - modeChangeCounter < TARGET_NONE_COUNT/2) trace(sensor);
else traceLeft(sensor);
// trace(sensor); // trace(sensor);
if(counter - modeChangeCounter < TARGET_NONE_COUNT/2) trace(sensor);
else traceLeft(sensor); //カーブの後半は加速したい
if(counter < LAP_COUNT) lance(30); //1周目は円筒標的をねらいたい if(counter < LAP_COUNT) lance(30); //1周目は円筒標的をねらいたい
else lance(-30); else lance(-30);
if(counter - modeChangeCounter > TARGET_NONE_COUNT){ if(counter - modeChangeCounter > TARGET_NONE_COUNT){
@ -339,17 +352,17 @@ void loop(){
mode = MODE_TRACE; mode = MODE_TRACE;
} }
break; break;
case MODE_TARGET_CYLINDER: //円筒標的 // case MODE_TARGET_CYLINDER: //円筒標的
digitalWrite(PIN_LED, HIGH); // digitalWrite(PIN_LED, HIGH);
handle(0); // handle(0);
lance(-90); // lance(-90);
motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT); // motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
if(sensor&MASK_CHECK_MARKER == 0b10000001){ // if(sensor&MASK_CHECK_MARKER == 0b10000001){
modeChangeCounter = counter; // modeChangeCounter = counter;
}else if( counter - modeChangeCounter > LAP_COUNT/21 ){ // }else if( counter - modeChangeCounter > LAP_COUNT/21 ){
mode = MODE_STOP; // mode = MODE_STOP;
} // }
break; // break;
case MODE_STOP: //停止 case MODE_STOP: //停止
digitalWrite(PIN_LED, HIGH); digitalWrite(PIN_LED, HIGH);
motorL.mode(STOP); motorL.mode(STOP);