CatPot 2015-2016
Main Program
Dependencies: mbed AQM1602 HMC6352 PID
Diff: main_processing/strategy/strategy.cpp
- Revision:
- 45:c23f25c00d0d
- Parent:
- 43:f11f68918299
--- a/main_processing/strategy/strategy.cpp Fri Feb 05 14:04:07 2016 +0000
+++ b/main_processing/strategy/strategy.cpp Thu Feb 25 06:12:47 2016 +0000
@@ -78,6 +78,7 @@
}
uint8_t LineSign[3];
uint8_t LineFirst[2];
+uint8_t LinePriority[2];//0を後に，1を優先
void LineClear_A(void){LineSign[A_SPOT]=0;data.lnFlag[A_SPOT]=0;}
void LineClear_B(void){LineSign[B_SPOT]=0;data.lnFlag[B_SPOT]=0;}
void LineClear_C(void){LineSign[C_SPOT]=0;data.lnFlag[C_SPOT]=0;}
@@ -257,7 +258,7 @@
uint8_t LineStop[2];
uint8_t IrRange[4];
uint8_t static LineBind[4];
- uint8_t static spi_count;
+ //buint8_t static spi_count;
if(data.KickOffFlag==1){
LineBind[0]=0;
LineBind[1]=0;
@@ -275,9 +276,13 @@
HmcResetFlag = 0;
PingFlag = 0;
- spi_count=0;
+ //spi_count=0;
+
+ RN42_Reset=0;
+ data.KickFlag = 0;
data.KickOffFlag=0;
+ //while((Sw[2].read()==1)&&(Sw[3].read()==1));//押して離すとスタート
}
if(data.IrFlag==1){
/*spi_count++;
@@ -321,8 +326,23 @@
vx = ir_x;
vy = ir_y;
+ if((data.irPosition==10)&&(vy>0)){
+ vy += 0;//前進加速
+ }
if((data.irPosition==11)&&(vy>0)){
- vy += 10;//前進加速
+ vy += 0;//前進加速
+ if(data.KickFlag==1){
+ DriveSolenoid();
+ }
+ }
+ if((data.irPosition==12)&&(vy>0)){
+ vy += 0;//前進加速
+ }
+ if((data.irPosition==1)&&(vy>0)){
+ vy += 0;//前進加速
+ }
+ if((data.irPosition==2)&&(vy>0)){
+ vy += 0;//前進加速
}
/*if((data.irPosition==17)&&(data.motorlog[X_AXIS]>0)){
vx *= -1.0;//背後回り込みの左右判断
@@ -331,28 +351,38 @@
if((data.irPosition==17)&&(data.ping[L_PING]>data.ping[R_PING])){
vx *= -1.0;//背後回り込みの左右判断
}
-
+ /*
+ if((data.InputPID<(REFERENCE-30))||(data.InputPID>(REFERENCE+30))){
+ vx = vx*(0.75);
+ vy = vy*(0.75);
+ }
+ */
//Lineを踏み始めた方向を調べる
- LineDir[A_SPOT] = (vx>0)&&((data.lnFlag[A_SPOT]==1)&&(1 ))&&(LineFirst[X_AXIS] == A_SPOT);
+ /*LineDir[A_SPOT] = (vx>0)&&((data.lnFlag[A_SPOT]==1)&&(1 ))&&(LineFirst[X_AXIS] == A_SPOT);
LineDir[B_SPOT] = (vx<0)&&((data.lnFlag[B_SPOT]==1)&&(1 ))&&(LineFirst[X_AXIS] == B_SPOT);
LineDir[C_SPOT] = (vy<0)&&((data.lnFlag[C_SPOT]==1)&&(1 ))&&(LineFirst[Y_AXIS] == C_SPOT);
- LineDir[AB_SPOT]= (vy>0)&&((data.lnFlag[A_SPOT]==1)&&(data.lnFlag[B_SPOT]==1))&&(LineFirst[Y_AXIS] == AB_SPOT);
+ LineDir[AB_SPOT]= (vy>0)&&((data.lnFlag[A_SPOT]==1)&&(data.lnFlag[B_SPOT]==1))&&(LineFirst[Y_AXIS] == AB_SPOT);*/
+
+ LineDir[A_SPOT] = (vx>0)&&((LineSign[A_SPOT]==1)&&(1 ))&&(LineFirst[X_AXIS] == A_SPOT);
+ LineDir[B_SPOT] = (vx<0)&&((LineSign[B_SPOT]==1)&&(1 ))&&(LineFirst[X_AXIS] == B_SPOT);
+ LineDir[C_SPOT] = (vy<0)&&((LineSign[C_SPOT]==1)&&(1 ))&&(LineFirst[Y_AXIS] == C_SPOT);
+ LineDir[AB_SPOT]= (vy>0)&&((LineSign[A_SPOT]==1)&&(LineSign[B_SPOT]==1))&&(LineFirst[Y_AXIS] == AB_SPOT);
//Irボールの方向
//strict
- /*
+
IrRange[A_SPOT] = ((18<=data.irPosition)&&(data.irPosition<=19))||((8<=data.irPosition)&&(data.irPosition<=10))
||((6<=data.irPosition)&&(data.irPosition<=7))||(( 1)&&(data.irPosition<=1));
IrRange[B_SPOT] = ((12<=data.irPosition)&&(data.irPosition<=16))||((2<=data.irPosition)&&(data.irPosition<=5));
IrRange[C_SPOT] = ((15<=data.irPosition)&&(data.irPosition<=19))||((4<=data.irPosition)&&(data.irPosition<=7));
IrRange[AB_SPOT] = ((9<=data.irPosition)&&(data.irPosition<=13))||(( 1)&&(data.irPosition<=3));
- */
+
//sweet
- IrRange[A_SPOT] = ((19<=data.irPosition)&&(data.irPosition<=19))||((8<=data.irPosition)&&(data.irPosition<=9))
+ /*IrRange[A_SPOT] = ((19<=data.irPosition)&&(data.irPosition<=19))||((8<=data.irPosition)&&(data.irPosition<=9))
||((6<=data.irPosition)&&(data.irPosition<=7))||(( 1)&&(data.irPosition<=1));
IrRange[B_SPOT] = ((13<=data.irPosition)&&(data.irPosition<=15))||((2<=data.irPosition)&&(data.irPosition<=5));
IrRange[C_SPOT] = ((16<=data.irPosition)&&(data.irPosition<=18))||((4<=data.irPosition)&&(data.irPosition<=7));
- IrRange[AB_SPOT] = ((10<=data.irPosition)&&(data.irPosition<=12))||(( 1)&&(data.irPosition<=3));
+ IrRange[AB_SPOT] = ((10<=data.irPosition)&&(data.irPosition<=12))||(( 1)&&(data.irPosition<=3));*/
//none
if(data.irNotice==IR_NONE){
IrRange[A_SPOT] = 0;
@@ -366,8 +396,8 @@
LineBind[C_SPOT] = (IrRange[C_SPOT]==1)&&((LineDir[C_SPOT]==1)||(LineBind[C_SPOT]==1));
LineBind[AB_SPOT] = (IrRange[AB_SPOT]==1)&&((LineDir[AB_SPOT]==1)||(LineBind[AB_SPOT]==1));
- LineStop[X_AXIS] = 1;//(LineBind[A_SPOT]==0)*(LineBind[B_SPOT]==0);
- LineStop[Y_AXIS] = 1;//(LineBind[C_SPOT]==0)*(LineBind[AB_SPOT]==0);
+ LineStop[X_AXIS] = (LineBind[A_SPOT]==0)*(LineBind[B_SPOT]==0);
+ LineStop[Y_AXIS] = (LineBind[C_SPOT]==0)*(LineBind[AB_SPOT]==0);
//白線踏んでる
if(RawLineSignal>0){
@@ -416,9 +446,9 @@
LineForce[X_AXIS] = (LINE_RF*2)*((-1)*(LineReturn[A_SPOT]==1) + ( 1)*(LineReturn[B_SPOT]==1)) +
(LINE_RF*2)*(LineReturn[AB_SPOT]==1)*(LineReturn[A_SPOT]==0)*(LineReturn[B_SPOT]==0)
- *(( 1)*(data.ping[L_PING]<30) + (-1)*(data.ping[R_PING]<30)) +
+ *(( 1)*(data.ping[L_PING]<40) + (-1)*(data.ping[R_PING]<40)) +
(LINE_RF*2)*(LineReturn[C_SPOT]==1)*(LineReturn[A_SPOT]==0)*(LineReturn[B_SPOT]==0)
- *(( 1)*(data.ping[L_PING]<30) + (-1)*(data.ping[R_PING]<30));
+ *(( 1)*(data.ping[L_PING]<40) + (-1)*(data.ping[R_PING]<40));
LineForce[Y_AXIS] = (LINE_RF*2)*(( 1)*(LineReturn[C_SPOT]==1) + (-1)*(LineReturn[AB_SPOT]==1));
Line_timeout[A_SPOT].attach(&LineClear_A, LINE_DELAY);
@@ -438,6 +468,11 @@
vy,
vs
);
+ /*move(
+ 0,
+ 0,
+ 10
+ );*/
data.motorlog[X_AXIS] += vx;
data.motorlog[Y_AXIS] += vy;
if(data.MotorFlag==1){
@@ -447,12 +482,265 @@
return;
}
void modeAttack3(void){
- if(data.KickFlag==1){
- DriveSolenoid();
+ double ir_x, ir_y;
+ int vx,vy,vs, LineForce[2];
+ uint8_t LineDir[4];
+ uint8_t LineOn[4];
+ uint8_t LineReturn[4];
+ uint8_t LineStop[2];
+ uint8_t IrRange[4];
+ uint8_t static LineBind[4];
+ //buint8_t static spi_count;
+ if(data.KickOffFlag==1){
+ LineBind[0]=0;
+ LineBind[1]=0;
+ LineBind[2]=0;
+ LineBind[3]=0;
+ LineReverseFlag=0;
+
+ LineSign[A_SPOT]=0;
+ LineSign[B_SPOT]=0;
+ LineSign[C_SPOT]=0;
+
+ data.lnFlag[A_SPOT]=0;
+ data.lnFlag[B_SPOT]=0;
+ data.lnFlag[C_SPOT]=0;
+
+ LinePriority[X_AXIS]=0;
+ LinePriority[Y_AXIS]=0;
+
+ HmcResetFlag = 0;
+ PingFlag = 0;
+ //spi_count=0;
+
+ RN42_Reset=0;
+ data.KickFlag = 0;
+
+ data.KickOffFlag=0;
+ //while((Sw[2].read()==1)&&(Sw[3].read()==1));//押して離すとスタート
+ }
+ if(data.IrFlag==1){
+ /*spi_count++;
+ if(spi_count%10 == 0){
+ ReadPing();
+ }
+ else{
+ ReadIr();
+ }
+ if(spi_count==20) spi_count=0;
+ */
+ ReadIr();
+ data.IrFlag=0;
+ }
+ if(data.PidFlag==1){
+ PidUpdate();
+ data.PidFlag=0;
+ }
+ if(PingFlag==1){
+ ReadPing();
+ PingFlag=0;
+ }
+ /*
+ if(HmcResetFlag==1){
+ HmcReset();
+ HmcResetFlag=0;
+ }
+ */
+ ir_x = ir_move_val[data.irNotice][data.irPosition][IR_X];
+ ir_y = ir_move_val[data.irNotice][data.irPosition][IR_Y];
+ if(data.irPosition<8){
+ ir_x *= data.l_pow;
+ ir_y *= data.l_pow;
+ }
+ else{
+ ir_x *= data.s_pow;
+ ir_y *= data.s_pow;
+ }
+
+ //Lineを考慮していないIrのみの値
+ vx = ir_x;
+ vy = ir_y;
+
+ if((data.irPosition==10)&&(vy>0)){
+ vy += 15;//前進加速
+ }
+ if((data.irPosition==11)&&(vy>0)){
+ vy += 15;//前進加速
+ if(data.KickFlag==1){
+ DriveSolenoid();
+ }
+ }
+ if((data.irPosition==12)&&(vy>0)){
+ vy += 15;//前進加速
+ }
+ if((data.irPosition==1)&&(vy>0)){
+ vy += 25;//前進加速
+ }
+ if((data.irPosition==2)&&(vy>0)){
+ vy += 25;//前進加速
+ }
+ /*if((data.irPosition==17)&&(data.motorlog[X_AXIS]>0)){
+ vx *= -1.0;//背後回り込みの左右判断
+ }*/
+
+ if((data.irPosition==17)&&(data.ping[L_PING]>data.ping[R_PING])){
+ vx *= -1.0;//背後回り込みの左右判断
+ }
+ /*
+ if((data.InputPID<(REFERENCE-30))||(data.InputPID>(REFERENCE+30))){
+ vx = vx*(0.75);
+ vy = vy*(0.75);
+ }
+ */
+ //Lineを踏み始めた方向を調べる
+ /*LineDir[A_SPOT] = (vx>0)&&((data.lnFlag[A_SPOT]==1)&&(1 ))&&(LineFirst[X_AXIS] == A_SPOT);
+ LineDir[B_SPOT] = (vx<0)&&((data.lnFlag[B_SPOT]==1)&&(1 ))&&(LineFirst[X_AXIS] == B_SPOT);
+ LineDir[C_SPOT] = (vy<0)&&((data.lnFlag[C_SPOT]==1)&&(1 ))&&(LineFirst[Y_AXIS] == C_SPOT);
+ LineDir[AB_SPOT]= (vy>0)&&((data.lnFlag[A_SPOT]==1)&&(data.lnFlag[B_SPOT]==1))&&(LineFirst[Y_AXIS] == AB_SPOT);*/
+
+ LineDir[A_SPOT] = (vx>0)&&((LineSign[A_SPOT]==1)&&(1 ))&&(LineFirst[X_AXIS] == A_SPOT);
+ LineDir[B_SPOT] = (vx<0)&&((LineSign[B_SPOT]==1)&&(1 ))&&(LineFirst[X_AXIS] == B_SPOT);
+ LineDir[C_SPOT] = (vy<0)&&((LineSign[C_SPOT]==1)&&(1 ))&&(LineFirst[Y_AXIS] == C_SPOT);
+ LineDir[AB_SPOT]= (vy>0)&&((LineSign[A_SPOT]==1)&&(LineSign[B_SPOT]==1))&&(LineFirst[Y_AXIS] == AB_SPOT);
+
+ //Irボールの方向
+ //strict
+
+ IrRange[A_SPOT] = ((18<=data.irPosition)&&(data.irPosition<=19))||((8<=data.irPosition)&&(data.irPosition<=10))
+ ||((6<=data.irPosition)&&(data.irPosition<=7))||(( 1)&&(data.irPosition<=1));
+ IrRange[B_SPOT] = ((12<=data.irPosition)&&(data.irPosition<=16))||((2<=data.irPosition)&&(data.irPosition<=5));
+ IrRange[C_SPOT] = ((15<=data.irPosition)&&(data.irPosition<=19))||((4<=data.irPosition)&&(data.irPosition<=7));
+ IrRange[AB_SPOT] = ((9<=data.irPosition)&&(data.irPosition<=13))||(( 1)&&(data.irPosition<=3));
+
+ //sweet
+ /*IrRange[A_SPOT] = ((19<=data.irPosition)&&(data.irPosition<=19))||((8<=data.irPosition)&&(data.irPosition<=9))
+ ||((6<=data.irPosition)&&(data.irPosition<=7))||(( 1)&&(data.irPosition<=1));
+ IrRange[B_SPOT] = ((13<=data.irPosition)&&(data.irPosition<=15))||((2<=data.irPosition)&&(data.irPosition<=5));
+ IrRange[C_SPOT] = ((16<=data.irPosition)&&(data.irPosition<=18))||((4<=data.irPosition)&&(data.irPosition<=7));
+ IrRange[AB_SPOT] = ((10<=data.irPosition)&&(data.irPosition<=12))||(( 1)&&(data.irPosition<=3));*/
+ //none
+ if(data.irNotice==IR_NONE){
+ IrRange[A_SPOT] = 0;
+ IrRange[B_SPOT] = 0;
+ IrRange[C_SPOT] = 0;
+ IrRange[AB_SPOT] = 0;
+ }
+ //白線を踏み始めた方向とボールの方向が一致．(SelfHold)
+ LineBind[A_SPOT] = (IrRange[A_SPOT]==1)&&((LineDir[A_SPOT]==1)||(LineBind[A_SPOT]==1));
+ LineBind[B_SPOT] = (IrRange[B_SPOT]==1)&&((LineDir[B_SPOT]==1)||(LineBind[B_SPOT]==1));
+ LineBind[C_SPOT] = (IrRange[C_SPOT]==1)&&((LineDir[C_SPOT]==1)||(LineBind[C_SPOT]==1));
+ LineBind[AB_SPOT] = (IrRange[AB_SPOT]==1)&&((LineDir[AB_SPOT]==1)||(LineBind[AB_SPOT]==1));
+
+ LineStop[X_AXIS] = 1;//(LineBind[A_SPOT]==0)*(LineBind[B_SPOT]==0);
+ LineStop[Y_AXIS] = 1;//(LineBind[C_SPOT]==0)*(LineBind[AB_SPOT]==0);
+
+ //白線踏んでる
+ if(RawLineSignal>0){
+ LineOn[A_SPOT] = (LineSign[A_SPOT]==1) &&(LineFirst[X_AXIS]==A_SPOT);
+ LineOn[B_SPOT] = (LineSign[B_SPOT]==1) &&(LineFirst[X_AXIS]==B_SPOT);
+ LineOn[C_SPOT] = (LineSign[C_SPOT]==1) &&(LineFirst[Y_AXIS]==C_SPOT);
+ LineOn[AB_SPOT] = ((LineSign[A_SPOT]==1)&&(LineSign[B_SPOT]==1))&&(LineFirst[Y_AXIS]==AB_SPOT);
+ //外側に向かう力を消す．
+ //x
+ if(((LineOn[A_SPOT]==1)&&(vx>0))||((LineOn[B_SPOT]==1)&&(vx<0))){
+
+ if(LinePriority[Y_AXIS]==0){
+ LinePriority[X_AXIS]=1;
+ LinePriority[Y_AXIS]=0;
+ }
+ vx=0;
+ //yの力を加える．
+ if(vy>0){vy += 10;}
+ if(vy<0){vy -= 10;}
+ }
+ //y
+ if(((LineOn[C_SPOT]==1)&&(vy<0))||((LineOn[AB_SPOT]==1)&&(vy>0))){
+ if(LinePriority[X_AXIS]==0){
+ LinePriority[X_AXIS]=0;
+ LinePriority[Y_AXIS]=1;
+ }
+ if((LinePriority[X_AXIS]==1)&&(LineOn[AB_SPOT]==1)){
+ LinePriority[X_AXIS]=0;
+ LinePriority[Y_AXIS]=1;
+ }
+ vy=0;
+ }
+ //内側に向かう力を加える．
+ LineReturn[A_SPOT] = (LineOn[A_SPOT]==1);
+ /*if((LineReturn[A_SPOT]==1)&&(LineOn[AB_SPOT]==1)){
+ if(LineOn[C_SPOT]==0){
+ LineReturn[A_SPOT]=0;
+ }
+ else{
+ LineReturn[A_SPOT]=1;
+ }
+ }*/
+
+ LineReturn[B_SPOT] = (LineOn[B_SPOT]==1);
+ /*if((LineReturn[B_SPOT]==1)&&(LineOn[AB_SPOT]==1)){
+ if(LineOn[C_SPOT]==0){
+ LineReturn[B_SPOT]=0;
+ }
+ else{
+ LineReturn[B_SPOT]=1;
+ }
+ }*/
+ LineReturn[C_SPOT] = (LineOn[C_SPOT]==1);
+ /*if(LineReturn[C_SPOT]==1){
+ LineReturn[A_SPOT]=0;
+ LineReturn[B_SPOT]=0;
+ }*/
+ LineReturn[AB_SPOT] = (LineOn[AB_SPOT]==1);
+
+ LineForce[X_AXIS] = (LINE_RF*2)*(LinePriority[X_AXIS])*((-1)*(LineReturn[A_SPOT]==1) + ( 1)*(LineReturn[B_SPOT]==1)) +
+ (LINE_RF*2)*(LineReturn[AB_SPOT]==1)*(( 1)*(data.ping[L_PING]<40) + (-1)*(data.ping[R_PING]<40)) +
+ (LINE_RF*2)*(LineReturn[C_SPOT]==1)*(( 1)*(data.ping[L_PING]<40) + (-1)*(data.ping[R_PING]<40));
+ LineForce[Y_AXIS] = (LINE_RF*2)*(LinePriority[Y_AXIS])*(( 1)*(LineReturn[C_SPOT]==1) + (-1)*(LineReturn[AB_SPOT]==1));
+ /*
+ LineForce[X_AXIS] = (LINE_RF*2)*((-1)*(LineReturn[A_SPOT]==1) + ( 1)*(LineReturn[B_SPOT]==1)) +
+ (LINE_RF*2)*(LineReturn[AB_SPOT]==1)*(LineReturn[A_SPOT]==0)*(LineReturn[B_SPOT]==0)
+ *(( 1)*(data.ping[L_PING]<30) + (-1)*(data.ping[R_PING]<30)) +
+ (LINE_RF*2)*(LineReturn[C_SPOT]==1)*(LineReturn[A_SPOT]==0)*(LineReturn[B_SPOT]==0)
+ *(( 1)*(data.ping[L_PING]<30) + (-1)*(data.ping[R_PING]<30));
+ LineForce[Y_AXIS] = (LINE_RF*2)*(( 1)*(LineReturn[C_SPOT]==1) + (-1)*(LineReturn[AB_SPOT]==1));
+ */
+ Line_timeout[A_SPOT].attach(&LineClear_A, LINE_DELAY);
+ Line_timeout[B_SPOT].attach(&LineClear_B, LINE_DELAY);
+ Line_timeout[C_SPOT].attach(&LineClear_C, LINE_DELAY);
+ }
+ else{
+ LineForce[X_AXIS] = 0;
+ LineForce[Y_AXIS] = 0;
+
+ LinePriority[X_AXIS]=0;
+ LinePriority[Y_AXIS]=0;
+ }
+
+ vx = vx*LineStop[X_AXIS] + LineForce[X_AXIS];
+ vy = vy*LineStop[Y_AXIS] + LineForce[Y_AXIS];
+ vs = data.OutputPID;
+ move(
+ vx,
+ vy,
+ vs
+ );
+ /*move(
+ 10,
+ 0,
+ 0
+ );*/
+ data.motorlog[X_AXIS] += vx;
+ data.motorlog[Y_AXIS] += vy;
+ if(data.MotorFlag==1){
+ tx_motor();
+ data.MotorFlag=0;
}
return;
}
void modeAttack4(void){
+ if(data.KickFlag==1){
+ DriveSolenoid();
+ }
return;
}
void modeAttack5(void){