ジャパンオープン用のメインプログラム
Dependencies: mbed AQM1602 HMC6352 PID
Diff: main_processing/strategy/strategy.cpp
- Revision:
- 11:3efae754e6ef
- Parent:
- 10:6df631c39f9b
- Child:
- 12:bee8f883c33a
--- a/main_processing/strategy/strategy.cpp Thu Mar 10 03:03:50 2016 +0000 +++ b/main_processing/strategy/strategy.cpp Thu Mar 10 08:26:33 2016 +0000 @@ -728,12 +728,124 @@ } return; } +double LineJudgeX(double x){ + const double static LineDecline[5] = {1, .75, .5, .375, 0}; + const uint8_t static WhiteToWall = 30; + uint8_t LineState; + uint8_t LnRaw; + uint8_t LnHold; + + LnRaw = LineRaw; + LnHold = LineHold; + + //line + LnRaw = LineRaw; + LnHold = LineHold; + /* + if((LineRaw==0)){ + if((data.ping[L_PING]>60)&&(data.ping[R_PING]>60)){ + LineLiberate(); + } + }*/ + + + + if(LnHold==7){ + if(LnRaw>0){ + //場外間際 + LineState = 3; + } + else{//LnRaw==0 + //powerVoid & restore + //場外 + LineState = 4; + } + } + else if(LnHold>0){ + if(LnRaw>0){ + //踏んでるけどまだ出てない + LineState = 4; + } + else{//LnRaw==0 + //線をまたいでいるか,中途半端に線を踏んだあと復帰したか + LineState = 4; + //ping&reset + //timeout&reset + } + } + //else if(LnHold==0){...maxpower + + if(x>=0){ + if(data.ping[R_PING]<WhiteToWall) return LineDecline[LineState]; + else return 1; + } + else{ + if(data.ping[L_PING]<WhiteToWall) return LineDecline[LineState]; + else return 1; + } +} +void LineJudgeSlow(double *x, double *y){ + const double static LineDecline[5] = {1, .75, .5, .375, 0}; + const uint8_t static WhiteToWall[2] = {30, 30}; + uint8_t LineState; + + //line + LineState = 0; + if(data.lnHold==7){ + if(data.lnRaw>0){ + //場外間際...減衰 + LineState = 3; + } + else{//data.lnRaw==0 + //場外...出力完全無効+反発 + //powerVoid & restore + LineState = 4; + } + } + else if(data.lnHold>0){ + if(data.lnRaw>0){ + //踏んでるけどまだ出てない...減衰 + LineState = 2; + //timeout&reset...detach + } + else{//data.lnRaw==0 + //線をまたいでいるか,中途半端に線を踏んだあと復帰したか...減衰 + LineState = 1; + //if((data.ping[L_PING]>40)&&(data.ping[R_PING]>40)) LineLiberate(); + //ping&reset + //timeout&reset + } + } + //else if(data.lnHold==0){...maxpower + + if(*x>=0){ + if(data.ping[R_PING]<WhiteToWall[X_PING]) (*x)=LineDecline[LineState]; + else *x=1; + } + else{ + if(data.ping[L_PING]<WhiteToWall[X_PING]) (*x)=LineDecline[LineState]; + else *x=1; + } + + if(*y>=0){ + if(data.ping[F_PING]<WhiteToWall[Y_PING]) (*y)=LineDecline[LineState]; + else *y=1; + } + else{ + if(data.ping[B_PING]<WhiteToWall[Y_PING]) (*y)=LineDecline[LineState]; + else *y=1; + } +} +void LineJudgeReturn(int8_t *x, int8_t *y); +void LineJudgeReset(void); void modeAttack4(void){ double ir_x_dir, ir_y_dir; - double ir_x_turn; - double ir_y_turn; + double ir_x_turn, ir_y_turn; + double ir_x, ir_y; uint8_t ir_pow; int vx,vy,vs; + + if(sys.KickOffFlag==1){ sys.IrBlind=0; @@ -748,6 +860,8 @@ if(sys.UswFlag==1){ReadPing();sys.UswFlag=0;} if(sys.UswFlag2==1){ReadPing2();sys.UswFlag2=0;} //if(sys.KickFlag==1){DriveSolenoid();} + data.lnRaw = LineRaw; + data.lnHold = LineHold; ir_x_dir = ir_move_val[0][data.irNotice][data.irPosition][IR_X_DIR]; ir_y_dir = ir_move_val[0][data.irNotice][data.irPosition][IR_Y_DIR]; @@ -767,11 +881,15 @@ } if(sys.IrBlind==1) data.irNotice=IR_NONE; - //if((LineHold==7) + + - vx = ir_pow*(ir_x_dir + ir_x_turn); - vy = ir_pow*(ir_y_dir + ir_y_turn); + ir_x = (ir_x_dir + ir_x_turn); + ir_y = (ir_y_dir + ir_y_turn); + + vx = ir_pow*ir_x; + vy = ir_pow*ir_y; vs = cmps_set.OutputPID; move( vx,