CatPot 2015-2016
せとうちオープン用のメインプログラム
Dependencies: mbed AQM1602 HMC6352 PID
Diff: main_processing/strategy/strategy.cpp
- Revision:
- 0:b910276f9da2
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main_processing/strategy/strategy.cpp Sun May 01 06:30:37 2016 +0000
@@ -0,0 +1,653 @@
+#include "mbed.h"
+#include "extern.h"
+
+//Atk
+void modeAttack4(void){
+ //irの値に影響するモーター補正値
+ //uint8_t ir_pow;
+ uint8_t ir_pow_x, ir_pow_y;
+ double ir_x_dir, ir_y_dir;
+ double ir_x_turn, ir_y_turn;
+ double ir_x, ir_y;
+ //lineの値に影響するモーター補正値
+ double LineSlowPower[2];
+ double LineReturnPower[2];
+
+ //モーターの出力値
+ int vx,vy,vs;
+
+ ////初期値を決める等
+ if(sys.KickOffFlag==1){
+ /*
+ ///Blindシリーズ．1を代入であらゆるセンサーなどの値を反映させないようにする．
+ sys.IrBlind=0;
+ sys.LineBlind=0;
+ sys.PingBlind=0;
+
+ sys.HomeBlind=1;
+ sys.DriBlind=1;
+ //defence
+ sys.DefenceFlag=0;
+ */
+
+
+ //Kick
+ sys.KickStopFlag=0;
+ //Ir
+ sys.ir_pow_table = 0;
+ //Line
+ //data.lnCorner[L_LINE]=data.lnCorner[R_LINE]=data.lnCorner[F_LINE]=data.lnCorner[B_LINE]=LINE_EMPTY;
+ data.lnRepeat = 0;
+ data.lnStay[X_LINE]=data.lnStay[Y_LINE]=1;
+ data.lnStayNow[X_LINE]=data.lnStayNow[Y_LINE]=0;
+ data.lnStop[X_LINE]=data.lnStop[Y_LINE]=1;
+
+ //data.FieldSpot = LINE_INSIDE;
+ //LineLiberate();
+ //LineRankClear();
+
+ //JSO2
+ LineLiberate();
+
+ LineRawRankClear();
+
+ //data.lnRawMemory[A_SPOT]=0;
+ //data.lnRawMemory[B_SPOT]=0;
+ //data.lnRawMemory[C_SPOT]=0;
+
+ data.lnRawReturn=0;
+
+ LineRawLogReset();
+ //backhome
+ sys.HomeStayFlag[X_PING]=0;
+ sys.HomeStayFlag[Y_PING]=0;
+ //pid
+ sys.TurnStopFlag=0;
+ cmps_set.AtkDeg=0;
+ cmps_set.HoldDeg=0;
+ cmps_set.GoalDeg=0;
+ //ドリブラー
+ sys.BallHoldJudgeFlag=0;
+ sys.BallHoldFlag=0;
+
+ //初期値設定の終了
+ sys.KickOffFlag=0;
+ }
+ ////DataRetrieve
+ if(sys.InfoFlag==1){ReadInfo();sys.InfoFlag=0;}
+ data.lnRaw = LineRaw;
+ data.lnHold = LineHold;
+ data.ball = ReadBall();
+
+ //ボールがなければ自分のゴールに戻る
+ if(data.irNotice==IR_NONE){
+
+ cmps_set.GoalDeg=0;
+ sys.BackHomeFlag=(sys.HomeBlind==0);
+ }
+ else{
+ sys.BackHomeFlag=0;
+ sys.HomeStayFlag[X_PING]=0;
+ sys.HomeStayFlag[Y_PING]=0;
+ }
+ //回り込みの値を代入
+ if(sys.DefenceFlag==1){
+
+ if((data.irNotice==IR_NONE)||(data.irNotice==IR_FAR)){
+
+ if((data.ping[L_PING]>=50)&&(data.ping[R_PING]>=50)){
+ ir_x_dir = ir_move_val[5][data.irNotice][data.irPosition][IR_X_DIR];
+ ir_x_turn = ir_move_val[5][data.irNotice][data.irPosition][IR_X_TURN];
+
+ if(data.ping[B_PING]>=10){
+ ir_y_dir = -1;
+ ir_y_turn = 0;
+ }
+ else{
+ ir_y_dir = 0;
+ ir_y_turn = 0;
+ }
+ }
+ else if((data.ping[L_PING]>=50)&&(data.ping[R_PING]<50)){
+ ir_x_dir = -1;
+ ir_x_turn = 0;
+
+ if(data.ping[B_PING]>=45){
+ ir_y_dir = -1;
+ ir_y_turn = 0;
+ }
+ else{
+ ir_y_dir = 0;
+ ir_y_turn = 0;
+ }
+ }
+ else if((data.ping[L_PING]<50)&&(data.ping[R_PING]>=50)){
+ ir_x_dir = 1;
+ ir_x_turn = 0;
+
+ if(data.ping[B_PING]>=45){
+ ir_y_dir = -1;
+ ir_y_turn = 0;
+ }
+ else{
+ ir_y_dir = 0;
+ ir_y_turn = 0;
+ }
+ }
+ else if((data.ping[L_PING]<50)&&(data.ping[R_PING]<50)){
+ ir_x_dir = 0;
+ ir_x_turn = 0;
+
+ if(data.ping[B_PING]>=45){
+ ir_y_dir = -1;
+ ir_y_turn = 0;
+ }
+ else{
+ ir_y_dir = 0;
+ ir_y_turn = 0;
+ }
+ }
+ }
+ else{
+
+ ir_x_dir = ir_move_val[5][data.irNotice][data.irPosition][IR_X_DIR];
+ ir_x_turn = ir_move_val[5][data.irNotice][data.irPosition][IR_X_TURN];
+
+ if((data.ping[B_PING]>=5)&&(data.ping[R_PING]>=50)&&(data.ping[L_PING]>=50)){
+ ir_y_dir = -1;
+ ir_y_turn = 0;
+ }
+ else{
+ ir_y_dir = 0;
+ ir_y_turn = 0;
+ }
+ }
+ /*
+ if((data.ping[L_PING]>=50)&&(data.ping[R_PING]>=50)&&(data.ping[B_PING]>10)){
+ ir_y_dir = -1;
+ ir_y_turn = 0;
+
+ if(data.ping[B_PING]>=10){
+ ir_y_dir = -1;
+ ir_y_turn = 0;
+ }
+ else{
+ ir_y_dir = 0;
+ ir_y_turn = 0;
+ }
+
+ ir_x_dir = ir_move_val[2][data.irNotice][data.irPosition][IR_X_DIR];
+ ir_x_turn = ir_move_val[2][data.irNotice][data.irPosition][IR_X_TURN];
+ }
+ else{
+
+ }
+ */
+ /*
+ if((data.ping[B_PING]>=45)&&(data.irNotice!=IR_NONE)&&(15<=data.irPosition)&&(data.irPosition<=19)){
+ //sys.ir_pow_table=1;
+ ir_x_dir = 0;
+ ir_y_dir = -1;
+ ir_x_turn = 0;
+ ir_y_turn = 0;
+ }
+ else{
+ if(data.irValPhase[IR_SHORT]>=DIS_4){
+ sys.ir_pow_table=2;//直進
+ ir_x_dir = ir_move_val[2][data.irNotice][data.irPosition][IR_X_DIR];
+ ir_y_dir = ir_move_val[2][data.irNotice][data.irPosition][IR_Y_DIR];
+ ir_x_turn = ir_move_val[2][data.irNotice][data.irPosition][IR_X_TURN];
+ ir_y_turn = ir_move_val[2][data.irNotice][data.irPosition][IR_Y_TURN];
+ }
+ else{
+ if(data.irValPhase[IR_SHORT]<=DIS_1){
+ sys.ir_pow_table=0;//3
+ 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];
+ ir_x_turn = ir_move_val[0][data.irNotice][data.irPosition][IR_X_TURN];
+ ir_y_turn = ir_move_val[0][data.irNotice][data.irPosition][IR_Y_TURN];
+ }
+ else{
+ sys.ir_pow_table=0;
+ 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];
+ ir_x_turn = ir_move_val[0][data.irNotice][data.irPosition][IR_X_TURN];
+ ir_y_turn = ir_move_val[0][data.irNotice][data.irPosition][IR_Y_TURN];
+ }
+ }
+ }*/
+ }
+ else{
+ if((data.ping[B_PING]>40)&&(0)){
+ sys.ir_pow_table=0;
+ 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];
+ ir_x_turn = ir_move_val[0][data.irNotice][data.irPosition][IR_X_TURN];
+ ir_y_turn = ir_move_val[0][data.irNotice][data.irPosition][IR_Y_TURN];
+ }
+ else{
+ if((data.ping[B_PING]>60)&&(data.irValPhase[IR_SHORT]>=DIS_4)){
+ sys.ir_pow_table=2;//直進
+ ir_x_dir = ir_move_val[2][data.irNotice][data.irPosition][IR_X_DIR];
+ ir_y_dir = ir_move_val[2][data.irNotice][data.irPosition][IR_Y_DIR];
+ ir_x_turn = ir_move_val[2][data.irNotice][data.irPosition][IR_X_TURN];
+ ir_y_turn = ir_move_val[2][data.irNotice][data.irPosition][IR_Y_TURN];
+ }
+ else{
+ if(data.irValPhase[IR_SHORT]<=DIS_1){
+ sys.ir_pow_table=0;//3
+ 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];
+ ir_x_turn = ir_move_val[0][data.irNotice][data.irPosition][IR_X_TURN];
+ ir_y_turn = ir_move_val[0][data.irNotice][data.irPosition][IR_Y_TURN];
+ }
+ else{
+ sys.ir_pow_table=0;
+ 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];
+ ir_x_turn = ir_move_val[0][data.irNotice][data.irPosition][IR_X_TURN];
+ ir_y_turn = ir_move_val[0][data.irNotice][data.irPosition][IR_Y_TURN];
+ }
+ }
+ }
+ if((sys.HomeBlind==0)){
+ if(((data.irNotice==IR_CLOSE)||(data.irNotice==IR_CLOSER))&&(data.ping[B_PING]<60)&&
+ (
+ (data.irPosition==11)||
+ (data.irPosition==(ir_posi_s[(11-8+24+1)%12]))||
+ (data.irPosition==(ir_posi_s[(11-8+24-1)%12]))||
+ (data.irPosition==(ir_posi_s[(11-8+24+2)%12]))||
+ (data.irPosition==(ir_posi_s[(11-8+24-2)%12]))||
+ (data.irPosition==(ir_posi_s[(11-8+24+3)%12]))||
+ (data.irPosition==(ir_posi_s[(11-8+24-3)%12]))
+ )
+ ){
+ sys.ir_pow_table=2;//直進
+ ir_x_dir = ir_move_val[2][data.irNotice][data.irPosition][IR_X_DIR];
+ ir_y_dir = ir_move_val[2][data.irNotice][data.irPosition][IR_Y_DIR];
+ ir_x_turn = ir_move_val[2][data.irNotice][data.irPosition][IR_X_TURN];
+ ir_y_turn = ir_move_val[2][data.irNotice][data.irPosition][IR_Y_TURN];
+ }
+ }
+ }
+
+ //Irの検出値によって出力を調整
+ if(data.irNotice==IR_CLOSER){
+ //ir_pow = sys.s_pow;
+ ir_pow_x = ir_pow_y = sys.s_pow;
+ }
+ else if(data.irNotice==IR_CLOSE){
+ //ir_pow = sys.m_pow;
+ ir_pow_x = ir_pow_y = sys.m_pow;
+ }
+ else if(data.irNotice==IR_FAR){
+ //ir_pow = sys.l_pow;
+ ir_pow_x = ir_pow_y = sys.l_pow;
+ }
+ else{//data.irNotice==IR_NONE
+ //ir_pow = 0;
+ ir_pow_x = ir_pow_y = 0;
+ }
+
+ if(sys.DefenceFlag==1){
+ ir_pow_x = 30;
+ ir_pow_y = 30;
+ if(data.ping[L_PING]<=60){
+ ir_pow_y = 30;
+ }
+ if(data.ping[L_PING]<=40){
+ ir_pow_y = 25;
+ }
+ if(data.ping[L_PING]<=30){
+ ir_pow_y = 20;
+ }
+ if(data.ping[L_PING]<15){
+ ir_pow_y = 15;
+ }
+ if(data.ping[L_PING]<10){
+ ir_pow_y = 10;
+ }
+ }
+ ////ドリブラーdribbler
+ //ホールド判定hold
+ JudgeBallHolding();
+ //JudgeBallHold();
+ //sys.BallHoldFlag = data.ball;
+ //ドリブラー駆動
+ /*if((sys.TurnDriBlind==0)&&(data.lnRawOrderLog1[0]!=LINE_EMPTY)&&(data.lnRawOrder[0]==LINE_EMPTY)){
+ if(sys.TurnDriBlind==0){
+ if(
+ ((data.lnRawOrderLog1[0]==A_SPOT)&&(data.lnRawOrderLog1[1]==B_SPOT))||
+ ((data.lnRawOrderLog1[0]==B_SPOT)&&(data.lnRawOrderLog1[1]==A_SPOT))
+ ){
+ cmps_set.GoalDeg=0;
+ }
+ else if((data.lnRawOrderLog1[0]==A_SPOT)&&(1)){
+ cmps_set.GoalDeg=-30;
+ }
+ else if((data.lnRawOrderLog1[0]==B_SPOT)&&(1)){
+ cmps_set.GoalDeg=30;
+ }
+ else{
+ cmps_set.GoalDeg=0;
+ }
+ }
+ }*/
+ if((sys.TurnDriBlind==0)&&(/*data.lnRaw==0*/1)&&((data.irNotice==IR_CLOSER)||(data.irNotice==IR_CLOSE))&&(/*data.ping[B_PING]>90*/1)&&
+ (
+ (data.irPosition==11)||
+ (data.irPosition==(ir_posi_s[(11-8+24+1)%12]))||
+ (data.irPosition==(ir_posi_s[(11-8+24-1)%12]))||
+ (data.irPosition==(ir_posi_s[(11-8+24+2)%12]))||
+ (data.irPosition==(ir_posi_s[(11-8+24-2)%12]))
+ )
+
+ ){
+ if(sys.TurnDriBlind==0){
+ if(
+ ((data.ping[L_PING]<40)&&(data.ping[R_PING]>70))
+ ){
+ cmps_set.GoalDeg=60;
+ }
+ else if(
+ ((data.ping[R_PING]<40)&&(data.ping[L_PING]>70))
+ ){
+ cmps_set.GoalDeg=-60;
+ }
+ else{
+ cmps_set.GoalDeg=0;
+ }
+ }
+ }
+ if((sys.DriBlind==0)&&(data.irNotice==IR_CLOSER)&&((data.irPosition==10)||(data.irPosition==11)||(data.irPosition==12))){
+ sys.DribbleFlag=1;
+ if((sys.BallHoldFlag==1)||(data.ball==1)){
+ //ir_pow=20;
+ ir_pow_x = ir_pow_y = 20;
+ }
+ }
+ else{
+ sys.DribbleFlag=0;
+ cmps_set.HoldDeg=0;
+ }
+
+ ////Kick
+ /*if((data.ball==1)&&(data.irNotice==IR_CLOSER)&&((data.irPosition==10)||(data.irPosition==11)||(data.irPosition==12))){
+ DriveTurn();
+ }*/
+ /*if((data.ball==1)&&(data.irNotice==IR_CLOSER)&&((data.irPosition==10)||(data.irPosition==11)||(data.irPosition==12))){
+ DriveTurn();
+ DriveSolenoid();
+ }*/
+ if(
+ /*(sys.BallHoldFlag==1)&&
+ (data.ball==1)&&
+ (data.irNotice==IR_CLOSER)&&
+ ((data.irPosition==10)||(data.irPosition==11)||(data.irPosition==12))&&
+ (data.ping[B_PING]>100)&&
+ (
+ ((data.ping[L_PING]<60)&&(data.ping[R_PING]>60))||
+ ((data.ping[L_PING]>60)&&(data.ping[R_PING]<60))||
+ (data.lnRawOrderLog1[0]==A_SPOT)||
+ (data.lnRawOrder[0]==A_SPOT)||
+ (data.lnRawOrderLog1[0]==B_SPOT)||
+ (data.lnRawOrder[0]==B_SPOT)
+ )&&
+ (data.lnRaw==0)*/
+ (sys.DriBlind==0)&&
+ (data.ping[R_PING]<30)&&
+ (data.ping[L_PING]<30)
+ ){
+ //DriveTurn();
+ //DriveSolenoid();
+ }
+
+ //Irの細かい補正値の処理．基本的にLineの補正値を演算する処理が始まる前にIrのモーター出力値補正をすべて終わらせる．
+ //if(sys.IrBlind==1) ir_pow=0;
+ if(sys.IrBlind==1) ir_pow_x=ir_pow_y=0;
+ if((data.ping[R_PING]<data.ping[L_PING])&&(data.irPosition==17)&&(data.irNotice==IR_CLOSER)){
+ ir_x_turn = -ir_x_turn;
+ ir_y_turn = -ir_y_turn;
+ }
+
+ if((sys.BackHomeFlag==1)&&(sys.HomeBlind==0)){
+ ir_pow_x = ir_pow_y = 25;
+ //x
+ if((abs(data.ping[L_PING]-data.ping[R_PING])>20)&&(sys.HomeStayFlag[X_PING]==0)){
+ if(data.ping[L_PING]>data.ping[R_PING]){
+ ir_x = -1;
+ }
+ else{
+ ir_x = 1;
+ }
+ if((data.ping[L_PING]<WhiteToWallPlus[X_PING])&&(data.ping[R_PING]<WhiteToWallPlus[X_PING])){
+ ir_x = 0;
+ }
+ }
+ else{
+ ir_x = 0;
+ //
+ sys.HomeStayFlag[X_PING]=1;
+ }
+ //y
+ if((data.ping[B_PING]>60)&&(1)&&(sys.HomeStayFlag[Y_PING]==0)){
+ ir_y = -1;
+ /*if((abs(data.ping[L_PING]-data.ping[R_PING])>30)&&(1)){
+ ir_pow_y = 15;
+ }*/
+ }
+ else{
+ ir_y = 0;
+ /*
+ if((1)&&(data.ping[B_PING]>60)&&(data.ping[R_PING]>55)&&(data.ping[L_PING]>55)){
+ ir_y = -1;
+ ir_pow_y = 15;
+ }
+ else{
+ ir_y = 0;
+ }
+ */
+ //
+ sys.HomeStayFlag[Y_PING]=1;
+ }
+ }
+ else{
+ ir_x = (ir_x_dir + ir_x_turn);
+ ir_y = (ir_y_dir + ir_y_turn);
+ }
+ if((sys.HomeBlind==0)&&(data.irNotice==IR_FAR)){
+ if(ir_y>0){
+ ir_y=0;
+ ir_pow_x = ir_pow_y = 25;
+
+ if((data.ping[B_PING]>50)&&(1)){
+ ir_y = -1;
+ //ir_pow_y = 15;
+ }
+
+ }
+ //ir_pow_x = ir_pow_y = 25;
+ //if(ir_y>0) ir_y=0;
+ /*if((data.ping[B_PING]>40)&&(1)){
+ ir_y = -1;
+ //ir_pow_y = 15;
+ }*/
+ }
+ if(
+ (sys.DefenceFlag==1)&&
+ (ir_y>0)&&
+ (data.ping[B_PING]>60)&&
+ (data.ping[B_PING]<225)&&
+ (sys.BackHomeFlag==0)
+ ){
+ ir_pow_y=0;
+ }
+ ///dribbler
+ if((sys.DriBlind==0)&&(data.irNotice==IR_CLOSER)&&(/*sys.BallHoldFlag==1*/data.ball==1)&&((data.irPosition==10)||(data.irPosition==11)||(data.irPosition==12))){
+
+ if((1/*data.ping[L_PING]<60*/)&&(data.ping[R_PING]>90)){
+ //ir_x = 1;
+ }
+ else{
+ if(data.lnRawOrderLog1[0]==B_SPOT){
+ //ir_x = 1;
+ }
+ }
+ if((1/*data.ping[R_PING]<60*/)&&(data.ping[L_PING]>90)){
+ //ir_x = -1;
+ }
+ else{
+ if(data.lnRawOrderLog1[0]==A_SPOT){
+ //ir_x = -1;
+ }
+ }
+ if((data.ping[F_PING]<50)&&(data.ping[B_PING]>100)){
+ //ir_y = -1;
+ }
+ if(
+ ((data.ping[L_PING]<55)&&(data.ping[R_PING]<55))||
+ ((data.ping[L_PING]>55)&&(data.ping[R_PING]>55))
+ ){
+ //DriveSolenoid();
+ }
+ //x
+ if((abs(data.ping[L_PING]-data.ping[R_PING])>40)&&(data.lnRawOrderLog1[0]==A_SPOT)){
+ ir_x = -1;
+ }
+ else if((abs(data.ping[L_PING]-data.ping[R_PING])>40)&&(data.lnRawOrderLog1[0]==B_SPOT)){
+ ir_x = 1;
+ }
+ else if((abs(data.ping[L_PING]-data.ping[R_PING])>10)&&((data.ping[L_PING]+data.ping[R_PING])>80)){
+ if(data.ping[L_PING]>data.ping[R_PING]){
+ ir_x = -1;
+ }
+ else{
+ ir_x = 1;
+ }
+ }
+ else{
+ if((abs(data.ping[L_PING]-data.ping[R_PING])>10)&&(1)){
+ if(data.ping[L_PING]>data.ping[R_PING]){
+ ir_x = -1;
+ }
+ else{
+ ir_x = 1;
+ }
+ }
+ else{
+ ir_x = 0;
+ }
+
+ //ir_x = 0;
+ DriveSolenoid();
+ }
+ //y
+ if((data.ping[F_PING]<45)&&(abs(data.ping[L_PING]-data.ping[R_PING])>50)){
+ //ir_y = 0.75;
+ }
+ //kick
+ if((abs(data.ping[L_PING]-data.ping[R_PING])<30)&&(1)){
+ DriveSolenoid();
+ }
+ }
+ ////Lineセンサーの値を評価しモーターの出力補正値を演算
+ if(sys.LineBlind==1){
+
+ LineSlowPower[X_LINE] = 1.0;
+ LineSlowPower[Y_LINE] = 1.0;
+
+ LineReturnPower[X_LINE] = 0.0;
+ LineReturnPower[Y_LINE] = 0.0;
+
+ data.lnStop[X_LINE] = 1;
+ data.lnStop[Y_LINE] = 1;
+
+ data.FieldSpot = LINE_INSIDE;
+ }
+ else{
+ /*
+ LineJudgeReset(ir_x, ir_y, &LineSlowPower[X_LINE], &LineSlowPower[Y_LINE]);
+ LineJudgeSlow(ir_x, ir_y, &LineSlowPower[X_LINE], &LineSlowPower[Y_LINE]);
+ LineJudgeReturn(ir_x*LineSlowPower[X_LINE], ir_y*LineSlowPower[Y_LINE], &LineReturnPower[X_LINE], &LineReturnPower[Y_LINE]);
+ */
+ LineJudgeReset3(ir_x, ir_y, &LineSlowPower[X_LINE], &LineSlowPower[Y_LINE]);
+ LineJudgeSlow2(ir_x, ir_y, &LineSlowPower[X_LINE], &LineSlowPower[Y_LINE], &ir_pow_x, &ir_pow_y);
+ LineJudgeReturn2(ir_x*LineSlowPower[X_LINE], ir_y*LineSlowPower[Y_LINE], &LineReturnPower[X_LINE], &LineReturnPower[Y_LINE]);
+ }
+ ////LEDデバッグ
+ //if(data.FieldSpot==LINE_OUTSIDE) LED = 0x9;
+ //if(data.FieldSpot==LINE_INSIDE) LED = 0x6;
+ //LED = ((data.lnRawMemory[0]!=0)<<2) | ((data.lnRawMemory[1]!=0)<<1) | ((data.lnRawMemory[2]!=0)<<0);
+ //LED = ((data.lnRawOrder[0]!=LINE_EMPTY)<<2) | ((data.lnRawOrder[1]!=LINE_EMPTY)<<1) | ((data.lnRawOrder[2]!=LINE_EMPTY)<<0);
+ //LED = ((data.lnOrder[0]!=LINE_EMPTY)<<2) | ((data.lnOrder[1]!=LINE_EMPTY)<<1) | ((data.lnOrder[2]!=LINE_EMPTY)<<0);
+
+ //LED = sys.BallHoldFlag*15;
+ if(sys.BallHoldFlag==0) LED=15;
+ else LED=10;
+ /*
+ if((abs(data.ping[L_PING]-data.ping[R_PING])>10)&&((data.ping[L_PING]+data.ping[R_PING])>80)){
+ if(data.ping[L_PING]>data.ping[R_PING]){
+ LED=9;
+ }
+ else{
+ LED=6;
+ }
+ }
+ else{
+ LED=15;
+ }*/
+
+ //if(data.ball==1) LED=15;
+ //else LED=10;
+
+ //if(data.lnRepeat>=1) LED=15;
+ //else LED=10;
+
+ //LED = 0xFF*(data.ping[B_PING]<30);
+
+ //LED = ((data.lnOrder[0]!=LINE_EMPTY)<<2) | ((data.lnOrder[1]!=LINE_EMPTY)<<1) | ((data.lnOrder[2]!=LINE_EMPTY)<<0);
+
+ //else LED = 0xA;
+ //LED = LineHold;
+
+
+ ////最終的なモーターの出力を演算
+ vx = (ir_pow_x*ir_x)*data.lnStop[X_LINE]*(data.lnStay[X_LINE])*LineSlowPower[X_LINE]*(sys.TurnStopFlag==0) + LineReturnPower[X_LINE];
+ vy = (ir_pow_y*ir_y)*data.lnStop[Y_LINE]*(data.lnStay[Y_LINE])*LineSlowPower[Y_LINE]*(sys.TurnStopFlag==0) + LineReturnPower[Y_LINE];
+ vs = cmps_set.OutputPID;
+ move(
+ vx,
+ vy,
+ vs
+ );
+ //モーターに信号を出力
+ if(sys.MotorFlag==1){tx_motor();sys.MotorFlag=0;}
+
+ if(sys.stopflag==1){
+ //コマンドモードに戻る際の処理
+ }
+ return;
+}
+
+void modeAttack5(void){
+ ////初期値を決める等
+ if(sys.KickOffFlag==1){
+ ///Blindシリーズ．1を代入であらゆるセンサーなどの値を反映させないようにする．
+ sys.IrBlind=0;
+ sys.LineBlind=0;
+ sys.PingBlind=0;
+
+ sys.HomeBlind=1;
+ sys.DriBlind=0;
+ sys.DriMotorBlind=0;
+ sys.TurnAtkBlind=1;
+ sys.TurnDriBlind=1;
+ sys.TurnHoldBlind=1;
+ sys.KickBlind=0;
+ //defence
+ sys.DefenceFlag=0;
+ //初期値設定の終了
+ //sys.KickOffFlag=0;
+ }
+ modeAttack4();
+}