CatPot 2015-2016
Main Program
Dependencies: mbed AQM1602 HMC6352 PID
Diff: main_processing/strategy/strategy.cpp
- Revision:
- 42:02aaa806d929
- Parent:
- 41:3cdcdec9c885
- Child:
- 43:f11f68918299
--- a/main_processing/strategy/strategy.cpp Thu Feb 04 11:43:53 2016 +0000
+++ b/main_processing/strategy/strategy.cpp Thu Feb 04 20:45:41 2016 +0000
@@ -60,8 +60,18 @@
}
return;
}
-
-
+uint8_t HmcResetFlag;
+void HmcReset(void){
+ RN42_Reset=1;
+ wait_us(100);
+ RN42_Reset=0;
+}
+uint8_t PingFlag;
+void ValidPing(void){
+ if(PingFlag==0){
+ PingFlag=1;
+ }
+}
uint8_t LineReverseFlag;
void LineReverse(void){
LineReverseFlag=0;
@@ -85,7 +95,7 @@
//
if((Line[A_SPOT].read()==1)||(1)) data.lnFlag[A_SPOT]=1;
//
- Line_timeout[A_SPOT].attach(&LineClear_A, 0.5);
+ Line_timeout[A_SPOT].attach(&LineClear_A, LINE_DELAY);
}
void LineCall_B(void){
//
@@ -101,7 +111,7 @@
//
if((Line[B_SPOT].read()==1)||(1)) data.lnFlag[B_SPOT]=1;
//
- Line_timeout[B_SPOT].attach(&LineClear_B, 0.5);
+ Line_timeout[B_SPOT].attach(&LineClear_B, LINE_DELAY);
}
void LineCall_C(void){
//
@@ -112,7 +122,7 @@
//
if((Line[C_SPOT].read()==1)||(1)) data.lnFlag[C_SPOT]=1;
//
- Line_timeout[C_SPOT].attach(&LineClear_C, 0.5);
+ Line_timeout[C_SPOT].attach(&LineClear_C, LINE_DELAY);
}
void modeAttack1(void){
double ir_x, ir_y;
@@ -213,9 +223,9 @@
vy=(LINE_RF*2)*( IrRange[C_SPOT] + (-1)*IrRange[AB_SPOT]);
/*vx=(LINE_RF*2)*((-1)*(LineFirst[X_AXIS] == A_SPOT) + (LineFirst[X_AXIS] == B_SPOT));
vy=(LINE_RF*2)*( (LineFirst[Y_AXIS] == C_SPOT) + (-1)*(LineFirst[Y_AXIS] == AB_SPOT));
- Line_timeout[A_SPOT].attach(&LineClear_A, 0.5);
- Line_timeout[B_SPOT].attach(&LineClear_B, 0.5);
- Line_timeout[C_SPOT].attach(&LineClear_C, 0.5);*/
+ 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{
vx=0;
@@ -223,9 +233,9 @@
}
}
if(RawLineSignal>0){
- Line_timeout[A_SPOT].attach(&LineClear_A, 0.5);
- Line_timeout[B_SPOT].attach(&LineClear_B, 0.5);
- Line_timeout[C_SPOT].attach(&LineClear_C, 0.5);
+ 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);
}
move(
vx,
@@ -239,6 +249,163 @@
return;
}
void modeAttack2(void){
+ double ir_x, ir_y;
+ int vx,vy,vs, LineForce[2];
+ uint8_t LineDir[4];
+ uint8_t LineOn[4];
+ uint8_t LineStop[2];
+ uint8_t IrRange[4];
+ uint8_t static LineBind[4];
+ 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;
+
+ HmcResetFlag = 0;
+ PingFlag = 0;
+
+ data.KickOffFlag=0;
+ }
+ if(data.IrFlag==1){
+ 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==11)&&(vy>0)){
+ vy += 10;//前進加速
+ }
+ 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;//背後回り込みの左右判断
+ }
+ */
+ //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);
+
+ //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));
+ //白線を踏み始めた方向とボールの方向が一致．(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] = (LineBind[A_SPOT]==0)*(LineBind[B_SPOT]==0);
+ LineStop[Y_AXIS] = (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))){
+ 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))){
+ vy=0;
+ }
+ //内側に向かう力を加える．
+ LineForce[X_AXIS] = (LINE_RF*2)*((-1)*(LineOn[A_SPOT]==1) + ( 1)*(LineOn[B_SPOT]==1));
+ LineForce[Y_AXIS] = (LINE_RF*2)*(( 1)*(LineOn[C_SPOT]==1) + (-1)*(LineOn[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;
+ }
+
+ vx = vx*LineStop[X_AXIS] + LineForce[X_AXIS];
+ vy = vy*LineStop[Y_AXIS] + LineForce[Y_AXIS];
+ vs = data.OutputPID;
+ move(
+ vx,
+ vy,
+ vs
+ );
+ data.motorlog[X_AXIS] += vx;
+ data.motorlog[Y_AXIS] += vy;
+ if(data.MotorFlag==1){
+ tx_motor();
+ data.MotorFlag=0;
+ }
+ return;
+}
+void modeAttack3(void){
+ if(data.KickFlag==1){
+ DriveSolenoid();
+ }
+ return;
+}
+void modeAttack4(void){
+ return;
+}
+void modeAttack5(void){
if(data.IrFlag==1){
ReadIr();
data.IrFlag=0;
@@ -255,191 +422,3 @@
return;
}
-void modeAttack3(void){
- if(data.KickFlag==1){
- DriveSolenoid();
- }
- return;
-}
-void modeAttack4(void){
- return;
-}
-void modeAttack5(void){
- return;
-}
-//Debug
-void modeDebug0(void){
- return;
-}
-void modeDebug1(void){//ChaseOnly NonLine
-int vx,vy,vs;
- if(data.IrFlag==1){
- ReadIr();
- data.IrFlag=0;
- }
- if(data.PidFlag==1){
- PidUpdate();
- data.PidFlag=0;
- }
-
- vx = ir_move_val[data.irNotice][data.irPosition][IR_X];
- vy = ir_move_val[data.irNotice][data.irPosition][IR_Y];
- vs = data.OutputPID;
- if(data.irPosition<8){
- vx *= data.l_pow;
- vy *= data.l_pow;
- }
- else{
- vx *= data.s_pow;
- vy *= data.s_pow;
- }
-
-
- move(vx, vy, vs);
-
- if(data.MotorFlag==1){
- tx_motor();
- data.MotorFlag=0;
- }
- return;
-}
-void modeDebug2(void){//LineRestoringForce
- int vx,vy,vs;
- uint8_t LineStop[2];
- int LineForce[2];
- if(data.IrFlag==1){
- ReadIr();
- data.IrFlag=0;
- }
- if(data.PidFlag==1){
- PidUpdate();
- data.PidFlag=0;
- }
- vx = ir_move_val[data.irNotice][data.irPosition][IR_X];
- vy = ir_move_val[data.irNotice][data.irPosition][IR_Y];
- vs = data.OutputPID;
- if(data.irPosition<8){
- vx *= data.l_pow;
- vy *= data.l_pow;
- }
- else{
- vx *= data.s_pow;
- vy *= data.s_pow;
- }
-
-
- LineStop[X_AXIS] = (!((vx>0)&&((data.lnFlag[A_SPOT]==1)&&(1))))*(!((vx<0)&&((data.lnFlag[B_SPOT]==1)&&(1))));
- LineStop[Y_AXIS] = (!((vy>0)&&((data.lnFlag[A_SPOT]==1)&&(data.lnFlag[B_SPOT]==1))))*(!((vy<0)&&((data.lnFlag[C_SPOT]==1)&&(1))));
-
- LineForce[X_AXIS] = LineForce[Y_AXIS] = 0;
- if(((vx>0)&&((data.lnFlag[A_SPOT]==1)&&(1)))){
- LineForce[X_AXIS] = -LINE_RF;
- }
- if(((vx<0)&&((data.lnFlag[B_SPOT]==1)&&(1)))){
- LineForce[X_AXIS] = LINE_RF;
- }
- if(((vy>0)&&((data.lnFlag[A_SPOT]==1)&&(data.lnFlag[B_SPOT]==1)))){
- LineForce[Y_AXIS] = -LINE_RF;
- }
- if(((vy<0)&&((data.lnFlag[C_SPOT]==1)&&(1)))){
- LineForce[Y_AXIS] = LINE_RF;
- }
-
- move(
- vx*LineStop[X_AXIS] + LineForce[X_AXIS],
- vy*LineStop[Y_AXIS] + LineForce[Y_AXIS],
- vs
- );
- if(data.MotorFlag==1){
- tx_motor();
- data.MotorFlag=0;
- }
- return;
-}
-void modeDebug3(void){//movesum
- int vx,vy,vs;
- uint8_t LineStop[2];
- static uint8_t moveLnFlag[4]={1, 1, 1, 1};
- static int moveLnlog[4];
- if(data.IrFlag==1){
- ReadIr();
- data.IrFlag=0;
- }
- if(data.PidFlag==1){
- PidUpdate();
- data.PidFlag=0;
- }
- vx = ir_move_val[data.irNotice][data.irPosition][IR_X];
- vy = ir_move_val[data.irNotice][data.irPosition][IR_Y];
- vs = data.OutputPID;
- if(data.irPosition<8){
- vx *= data.l_pow;
- vy *= data.l_pow;
- }
- else{
- vx *= data.s_pow;
- vy *= data.s_pow;
- }
-
- data.motorlog[X_AXIS] += vx;
- data.motorlog[Y_AXIS] += vy;
-
- if((data.lnFlag[A_SPOT]==1)&&(1)){
- moveLnFlag[A_SPOT]=0;
- moveLnlog[A_SPOT] = data.motorlog[X_AXIS];
- }
- if((data.lnFlag[B_SPOT]==1)&&(1)){
- moveLnFlag[B_SPOT]=0;
- moveLnlog[B_SPOT] = data.motorlog[X_AXIS];
- }
- if((data.lnFlag[C_SPOT]==1)&&(1)){
- moveLnFlag[C_SPOT]=0;
- moveLnlog[C_SPOT] = data.motorlog[Y_AXIS];
- }
- if((data.lnFlag[A_SPOT]==1)&&(data.lnFlag[B_SPOT]==1)){
- moveLnFlag[AB_SPOT]=0;
- moveLnlog[AB_SPOT] = data.motorlog[Y_AXIS];
- }
-
- if(((data.motorlog[X_AXIS]-moveLnlog[A_SPOT])<(-RELEASE_VAL))&&(moveLnFlag[A_SPOT]==0)) moveLnFlag[A_SPOT]=1;
- if(((data.motorlog[X_AXIS]-moveLnlog[B_SPOT])>(RELEASE_VAL))&&(moveLnFlag[B_SPOT]==0)) moveLnFlag[B_SPOT]=1;
- if(((data.motorlog[Y_AXIS]-moveLnlog[C_SPOT])>(RELEASE_VAL))&&(moveLnFlag[C_SPOT]==0)) moveLnFlag[C_SPOT]=1;
- if(((data.motorlog[Y_AXIS]-moveLnlog[AB_SPOT])<(-RELEASE_VAL))&&(moveLnFlag[AB_SPOT]==0)) moveLnFlag[AB_SPOT]=1;
-
- LineStop[X_AXIS] = (!((vx>0)&&(moveLnFlag[A_SPOT]==0)))*(!((vx<0)&&(moveLnFlag[B_SPOT]==0)));
- LineStop[Y_AXIS] = (!((vy>0)&&(moveLnFlag[C_SPOT]==0)))*(!((vy<0)&&(moveLnFlag[AB_SPOT]==0)));
-
- move(
- vx*LineStop[X_AXIS],
- vy*LineStop[Y_AXIS],
- vs
- );
- if(data.MotorFlag==1){
- tx_motor();
- data.MotorFlag=0;
- }
- return;
-}
-void modeDebug4(void){//solenoid
- if(data.KickFlag==1){
- DriveSolenoid();
- }
- return;
-}
-void modeDebug5(void){//cmpsTest
- if(data.IrFlag==1){
- ReadIr();
- data.IrFlag=0;
- }
- if(data.PidFlag==1){
- PidUpdate();
- data.PidFlag=0;
- }
- move(0,0,data.OutputPID);
- if(data.MotorFlag==1){
- tx_motor();
- data.MotorFlag=0;
- }
-
- return;
-}