コメントを追加
This commit is contained in:
parent
08ab4cd847
commit
440326dbe4
1 changed files with 94 additions and 81 deletions
|
|
@ -16,15 +16,13 @@
|
|||
#include <Servo.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_PARALLEL2 = 90 - 43;
|
||||
const int LANCE_ANGLE_LEFT_VERTICAL = 90 - 120;
|
||||
const int LANCE_ANGLE_RIGHT_VERTICAL = 90 - 57;
|
||||
const int LANCE_ANGLE_PARALLEL2 = 90 - 43; //度
|
||||
const int LANCE_ANGLE_LEFT_VERTICAL = 90 - 120; //度
|
||||
const int LANCE_ANGLE_RIGHT_VERTICAL = 90 - 57; //度
|
||||
|
||||
const byte SPEED_DEFAULT = 255;
|
||||
|
||||
|
|
@ -68,8 +66,8 @@ enum {
|
|||
MODE_TARGET_CYLINDER //円筒標的
|
||||
};
|
||||
|
||||
const byte MASK_MODE_TRACE = 0b01111110;
|
||||
const byte MASK_CHECK_MARKER = 0b10000001;
|
||||
const byte MASK_MODE_TRACE = 0b01111110;
|
||||
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){
|
||||
switch(sensor&MASK_MODE_TRACE){
|
||||
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){
|
||||
motorL.mode( leftMode, leftSpeed);
|
||||
motorR.mode(rightMode, rightSpeed);
|
||||
}
|
||||
|
||||
/* ステアリングを中央からdegree[度]だけ動かす
|
||||
<- - 0 + ->
|
||||
| |
|
||||
| |
|
||||
[]---{}---[]
|
||||
/ .\
|
||||
*/
|
||||
void handle(int degree){
|
||||
servoHandle.write(HANDLE_DEGREE_DEFAULT + degree);
|
||||
}
|
||||
|
||||
/* ランスを中央からdegree[度]だけ動かす
|
||||
<- - 0 + ->
|
||||
/ .\
|
||||
/ | \
|
||||
[]+--{}--+[]
|
||||
*/
|
||||
void lance(int degree){
|
||||
servoLance.write(LANCE_DEGREE_DEFAULT - degree);
|
||||
}
|
||||
|
||||
int lanceRead(){
|
||||
return LANCE_DEGREE_DEFAULT - servoLance.read();
|
||||
}
|
||||
|
||||
/* ロータリーエンコーダーの変化を見る
|
||||
todo:外部割り込み使ってみたい、、、
|
||||
*/
|
||||
|
|
@ -211,6 +221,7 @@ void count(){
|
|||
}
|
||||
}
|
||||
|
||||
/* マーカーを読んで、モードを返す */
|
||||
int checkMarker(byte sensor){
|
||||
int mode = MODE_TRACE;
|
||||
switch(sensor & MASK_CHECK_MARKER){
|
||||
|
|
@ -220,11 +231,11 @@ int checkMarker(byte sensor){
|
|||
break;
|
||||
case 0b00000001:
|
||||
modeChangeCounter = counter;
|
||||
if(counter%LAP_COUNT > LAP_COUNT/2){
|
||||
if(counter%LAP_COUNT > LAP_COUNT/2){ //後半は垂直標的をねらいたい
|
||||
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;
|
||||
}else if(counter%LAP_COUNT < LAP_COUNT/3){
|
||||
}else if(counter%LAP_COUNT < LAP_COUNT/3){ //次に、F的をねらいたい
|
||||
mode = MODE_TARGET_PARALLEL2;
|
||||
}else{
|
||||
mode = MODE_TARGET_RIGHT_VERTICAL;
|
||||
|
|
@ -238,6 +249,7 @@ int checkMarker(byte sensor){
|
|||
return mode;
|
||||
}
|
||||
|
||||
/* 前方センサーの初期化 */
|
||||
void sensorInit(){
|
||||
pinMode(PIN_SENSOR_0, INPUT);
|
||||
pinMode(PIN_SENSOR_1, INPUT);
|
||||
|
|
@ -249,6 +261,7 @@ void sensorInit(){
|
|||
pinMode(PIN_SENSOR_7, INPUT);
|
||||
}
|
||||
|
||||
/* センサーの状態を返す */
|
||||
byte sensorRead(){
|
||||
byte sensor = 0; //黒が1、白が0
|
||||
if(digitalRead(PIN_SENSOR_0)) sensor += 0b00000001;
|
||||
|
|
@ -308,7 +321,7 @@ void loop(){
|
|||
case MODE_TARGET_PARALLEL1: //平行標的1
|
||||
trace(sensor);
|
||||
lance(LANCE_ANGLE_PARALLEL1);
|
||||
if(counter - modeChangeCounter > 24){
|
||||
if(counter - modeChangeCounter > 24){ //マーカーから少し進んで、叩く
|
||||
lance(LANCE_ANGLE_PARALLEL1+8);
|
||||
mode = MODE_TRACE;
|
||||
}
|
||||
|
|
@ -316,7 +329,7 @@ void loop(){
|
|||
case MODE_TARGET_PARALLEL2: //平行標的2
|
||||
trace(sensor);
|
||||
lance(LANCE_ANGLE_PARALLEL2);
|
||||
if(counter - modeChangeCounter > 26){
|
||||
if(counter - modeChangeCounter > 26){ //マーカーから少し進んで、叩く
|
||||
lance(LANCE_ANGLE_PARALLEL2+8);
|
||||
mode = MODE_TRACE;
|
||||
}
|
||||
|
|
@ -324,14 +337,14 @@ void loop(){
|
|||
case MODE_TRACE:
|
||||
trace(sensor);
|
||||
mode = checkMarker(sensor);
|
||||
if(counter > LAP_COUNT*10){
|
||||
if(counter > LAP_COUNT*10){ //10周したら停止
|
||||
mode = MODE_STOP;
|
||||
}
|
||||
break;
|
||||
case MODE_LEFT: //左カーブ
|
||||
if(counter - modeChangeCounter < TARGET_NONE_COUNT/2) trace(sensor);
|
||||
else traceLeft(sensor);
|
||||
// trace(sensor);
|
||||
if(counter - modeChangeCounter < TARGET_NONE_COUNT/2) trace(sensor);
|
||||
else traceLeft(sensor); //カーブの後半は加速したい
|
||||
if(counter < LAP_COUNT) lance(30); //1周目は円筒標的をねらいたい
|
||||
else lance(-30);
|
||||
if(counter - modeChangeCounter > TARGET_NONE_COUNT){
|
||||
|
|
@ -339,17 +352,17 @@ void loop(){
|
|||
mode = MODE_TRACE;
|
||||
}
|
||||
break;
|
||||
case MODE_TARGET_CYLINDER: //円筒標的
|
||||
digitalWrite(PIN_LED, HIGH);
|
||||
handle(0);
|
||||
lance(-90);
|
||||
motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
|
||||
if(sensor&MASK_CHECK_MARKER == 0b10000001){
|
||||
modeChangeCounter = counter;
|
||||
}else if( counter - modeChangeCounter > LAP_COUNT/21 ){
|
||||
mode = MODE_STOP;
|
||||
}
|
||||
break;
|
||||
// case MODE_TARGET_CYLINDER: //円筒標的
|
||||
// digitalWrite(PIN_LED, HIGH);
|
||||
// handle(0);
|
||||
// lance(-90);
|
||||
// motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
|
||||
// if(sensor&MASK_CHECK_MARKER == 0b10000001){
|
||||
// modeChangeCounter = counter;
|
||||
// }else if( counter - modeChangeCounter > LAP_COUNT/21 ){
|
||||
// mode = MODE_STOP;
|
||||
// }
|
||||
// break;
|
||||
case MODE_STOP: //停止
|
||||
digitalWrite(PIN_LED, HIGH);
|
||||
motorL.mode(STOP);
|
||||
|
|
|
|||
Loading…
Add table
Reference in a new issue