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

update

Browse source
This commit is contained in:
Nebel 2013-10-05 22:22:46 +09:00
parent 0a6cfd416c
commit f728213817
1 changed files with 66 additions and 35 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

101
hanzo4.ino
View file

@ -8,14 +8,17 @@
#include <Servo.h>
#include "Motor.h"
const unsigned long LAP_DISTANCE = 28500; //ロータリーエンコーダーのカウント数/周
const unsigned long TURN_DISTANCE = 2000; //カーブ中でのカウント数
const unsigned long LAP_DISTANCE = 28510; //ロータリーエンコーダーのカウント数/周
const unsigned long TURN_DISTANCE = 4700; //カーブ中でのカウント数
const int LANCE_ANGLE_PARALLEL_1 = 55; //度
const int LANCE_ANGLE_PARALLEL_2 = 90 - 50; //度
const int LANCE_ANGLE_VERTICAL_L = 90 - 120; //度
const int LANCE_ANGLE_VERTICAL_R = 90 - 62; //度
const int LANCE_ANGLE_E = 45; //度
const int LANCE_ANGLE_F = 62; //度
const int LANCE_ANGLE_A = 29; //度
const int LANCE_ANGLE_B = -30; //度
const int LANCE_ANGLE_C = 32; //度
const int LANCE_ANGLE_D = -40; //度
//const unsigned char SPEED_DEFAULT = 0;
const unsigned char SPEED_DEFAULT = 127;
//const float STEERING_KP = 3.8;
@ -26,7 +29,7 @@ const unsigned int STEERING_MIN = (STEERING_CENTER-780); //us
const unsigned int STEERING_MAX = (STEERING_CENTER+780); //us
//Servo : SC-0352
const unsigned int LANCE_CENTER = 1450; //us
const unsigned int LANCE_CENTER = 1530; //us
const unsigned int LANCE_MIN = (LANCE_CENTER-780); //us
const unsigned int LANCE_MAX = (LANCE_CENTER+780); //us
@ -75,10 +78,12 @@ enum mode_t {
MODE_STOP, //停止
MODE_STRAIGHT, //まっすぐ進む
MODE_TURN, //左に曲がる
MODE_ATTACK_PARALLEL_1, //平行標的1
MODE_ATTACK_PARALLEL_2, //平行標的2
MODE_ATTACK_VERTICAL_R, //右垂直標的
MODE_ATTACK_VERTICAL_L, //左垂直標的
MODE_ATTACK_E, //平行標的1
MODE_ATTACK_F, //平行標的2
MODE_ATTACK_A, //右垂直標的
MODE_ATTACK_B, //左垂直標的
MODE_ATTACK_C, //右垂直標的
MODE_ATTACK_D, //左垂直標的
MODE_ATTACK_CYLINDER //円筒標的
}
mode;
@ -89,7 +94,7 @@ mode;
void trace(){
switch(sensor&MASK_LINE){
case 0b01000000:
steering(-22);
steering(-21);
motorL.speed(-speed/2);
motorR.speed(speed/2);
break;
@ -99,7 +104,7 @@ void trace(){
motorR.speed(speed/2);
break;
case 0b00100000:
steering(-18);
steering(-19);
motorL.speed(-speed/2);
motorR.speed(speed/2);
break;
@ -150,7 +155,7 @@ void trace(){
void modeSet(){
if(sensor == 0) errorCount++;
else errorCount = 0;
if(distance > LAP_DISTANCE*5 || errorCount > 10000){
if(distance > LAP_DISTANCE*5 || errorCount > 5000){
mode = MODE_STOP;
return;
}
@ -165,17 +170,27 @@ void modeSet(){
break;
case 0b00000001:
modeChangedDistance = distance;
if(distance%LAP_DISTANCE < 1429){ //まずは、E的をねらいたい
mode = MODE_ATTACK_PARALLEL_1;
}else if(distance%LAP_DISTANCE < 3333){ //次に、F的をねらいたい
mode = MODE_ATTACK_PARALLEL_2;
}else{ //後半は垂直標的をねらいたい
mode = MODE_ATTACK_VERTICAL_R;
if(distance%LAP_DISTANCE < LAP_DISTANCE/8){ //まずは、E的をねらいたい
mode = MODE_ATTACK_E;
}else if(distance%LAP_DISTANCE < LAP_DISTANCE/4){ //次に、F的をねらいたい
mode = MODE_ATTACK_F;
}else if(distance%LAP_DISTANCE > LAP_DISTANCE/2){ //後半は垂直標的をねらいたい
if(distance%LAP_DISTANCE < LAP_DISTANCE*0.6){
mode = MODE_ATTACK_A;
}else if(distance%LAP_DISTANCE < LAP_DISTANCE*0.85){
mode = MODE_ATTACK_C;
}
}
break;
case 0b10000000:
modeChangedDistance = distance;
if(distance%LAP_DISTANCE > 5000) mode = MODE_ATTACK_VERTICAL_L;
if(distance%LAP_DISTANCE < LAP_DISTANCE/2){
;
}else if(distance%LAP_DISTANCE < LAP_DISTANCE*0.80){
mode = MODE_ATTACK_B;
}else if(distance%LAP_DISTANCE < LAP_DISTANCE*0.85){
mode = MODE_ATTACK_D;
}
break;
}
}
@ -233,34 +248,50 @@ void loop(){
mode = MODE_STRAIGHT;
}
break;
case MODE_ATTACK_PARALLEL_1: //平行標的1
case MODE_ATTACK_E: //平行標的1
trace();
lance(LANCE_ANGLE_PARALLEL_1);
if(distance - modeChangedDistance > 1200){ //マーカーから少し進んで、叩く
lance(LANCE_ANGLE_PARALLEL_1 + 10);
lance(LANCE_ANGLE_E);
if(distance - modeChangedDistance > 1000){ //マーカーから少し進んで、叩く
lance(LANCE_ANGLE_E + 5);
mode = MODE_STRAIGHT;
}
break;
case MODE_ATTACK_PARALLEL_2: //平行標的2
case MODE_ATTACK_F: //平行標的2
trace();
lance(LANCE_ANGLE_PARALLEL_2);
if(distance - modeChangedDistance > 1200){ //マーカーから少し進んで、叩く
lance(LANCE_ANGLE_PARALLEL_2 + 10);
lance(LANCE_ANGLE_F);
if(distance - modeChangedDistance > 1000){ //マーカーから少し進んで、叩く
lance(LANCE_ANGLE_F + 5);
mode = MODE_STRAIGHT;
}
break;
case MODE_ATTACK_VERTICAL_R: //右垂直標
case MODE_ATTACK_A: //右垂直標
trace();
lance(LANCE_ANGLE_VERTICAL_R);
if(distance - modeChangedDistance > 300){ //マーカーから少し進んで、まっすぐに戻す
lance(LANCE_ANGLE_A);
if(distance - modeChangedDistance > 1500){ //マーカーから少し進んで、まっすぐに戻す
lance(0);
mode = MODE_STRAIGHT;
}
break;
case MODE_ATTACK_VERTICAL_L: //左垂直標
case MODE_ATTACK_C: //右垂直標
trace();
lance(LANCE_ANGLE_VERTICAL_L);
if(distance - modeChangedDistance > 300){ //マーカーから少し進んで、まっすぐに戻す
lance(LANCE_ANGLE_C);
if(distance - modeChangedDistance > 1500){ //マーカーから少し進んで、まっすぐに戻す
lance(0);
mode = MODE_STRAIGHT;
}
break;
case MODE_ATTACK_B: //左垂直標
trace();
lance(LANCE_ANGLE_B);
if(distance - modeChangedDistance > 1500){ //マーカーから少し進んで、まっすぐに戻す
lance(0);
mode = MODE_STRAIGHT;
}
break;
case MODE_ATTACK_D: //左垂直標
trace();
lance(LANCE_ANGLE_D);
if(distance - modeChangedDistance > 1500){ //マーカーから少し進んで、まっすぐに戻す
lance(0);
mode = MODE_STRAIGHT;
}