CatPot 2015-2016 / Mbed 2 deprecated CatPot_2v10_T_Main

ジャパンオープン用のメインプログラム

Dependencies:   mbed AQM1602 HMC6352 PID

main_processing/strategy/strategy.cpp@26:6ca88eeaa2b4, 2016-03-21 (annotated)

Committer:
lilac0112_1
Date:
Mon Mar 21 05:49:18 2016 +0000
Revision:
26:6ca88eeaa2b4
Parent:
25:a7460e23e02e
Child:
27:825c6835e3db
hard error has been declined.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
lilac0112_1 0:ea35c18c85fc 1 #include "mbed.h"
lilac0112_1 0:ea35c18c85fc 2 #include "extern.h"
lilac0112_1 0:ea35c18c85fc 3
lilac0112_1 0:ea35c18c85fc 4 //Atk
lilac0112_1 0:ea35c18c85fc 5 void modeAttack4(void){
lilac0112_1 24:34ef6379b0df 6 //irの値に影響するモーター補正値
lilac0112_1 25:a7460e23e02e 7 //uint8_t ir_pow;
lilac0112_1 25:a7460e23e02e 8 uint8_t ir_pow_x, ir_pow_y;
lilac0112_1 10:6df631c39f9b 9 double ir_x_dir, ir_y_dir;
lilac0112_1 11:3efae754e6ef 10 double ir_x_turn, ir_y_turn;
lilac0112_1 11:3efae754e6ef 11 double ir_x, ir_y;
lilac0112_1 24:34ef6379b0df 12 //lineの値に影響するモーター補正値
lilac0112_1 14:b510adcb6065 13 double LineSlowPower[2];
lilac0112_1 14:b510adcb6065 14 double LineReturnPower[2];
lilac0112_1 24:34ef6379b0df 15 //モーターの出力値
lilac0112_1 10:6df631c39f9b 16 int vx,vy,vs;
lilac0112_1 11:3efae754e6ef 17
lilac0112_1 24:34ef6379b0df 18 ////初期値を決める等
lilac0112_1 10:6df631c39f9b 19 if(sys.KickOffFlag==1){
lilac0112_1 24:34ef6379b0df 20 ///Blindシリーズ．1を代入であらゆるセンサーなどの値を反映させないようにする．
lilac0112_1 26:6ca88eeaa2b4 21 sys.IrBlind=1;
lilac0112_1 10:6df631c39f9b 22 sys.LineBlind=0;
lilac0112_1 10:6df631c39f9b 23 sys.PingBlind=0;
lilac0112_1 10:6df631c39f9b 24
lilac0112_1 26:6ca88eeaa2b4 25 sys.HomeBlind=1;
lilac0112_1 26:6ca88eeaa2b4 26 sys.DriBlind=1;
lilac0112_1 24:34ef6379b0df 27 //Kick
lilac0112_1 24:34ef6379b0df 28 sys.KickStopFlag=0;
lilac0112_1 24:34ef6379b0df 29 //Ir
lilac0112_1 18:3a42a931c95a 30 sys.ir_pow_table = 0;
lilac0112_1 24:34ef6379b0df 31 //Line
lilac0112_1 18:3a42a931c95a 32 data.lnCorner[L_LINE]=data.lnCorner[R_LINE]=data.lnCorner[F_LINE]=data.lnCorner[B_LINE]=LINE_EMPTY;
lilac0112_1 18:3a42a931c95a 33 data.lnRepeat = 0;
lilac0112_1 24:34ef6379b0df 34 data.lnStay[X_LINE]=data.lnStay[Y_LINE]=1;
lilac0112_1 18:3a42a931c95a 35 data.lnStop[X_LINE]=data.lnStop[Y_LINE]=1;
lilac0112_1 15:17502a27a60b 36 data.FieldSpot = LINE_INSIDE;
lilac0112_1 14:b510adcb6065 37 LineLiberate();
lilac0112_1 16:4fadb7a87497 38 LineRankClear();
lilac0112_1 25:a7460e23e02e 39 //backhome
lilac0112_1 25:a7460e23e02e 40 sys.HomeStayFlag[X_PING]=0;
lilac0112_1 25:a7460e23e02e 41 sys.HomeStayFlag[Y_PING]=0;
lilac0112_1 24:34ef6379b0df 42 //pid
lilac0112_1 24:34ef6379b0df 43 sys.TurnStopFlag=0;
lilac0112_1 25:a7460e23e02e 44 cmps_set.AtkDeg=0;
lilac0112_1 25:a7460e23e02e 45 cmps_set.HoldDeg=0;
lilac0112_1 24:34ef6379b0df 46 //ドリブラー
lilac0112_1 25:a7460e23e02e 47 sys.BallHoldJudgeFlag=0;
lilac0112_1 25:a7460e23e02e 48 sys.BallHoldFlag=0;
lilac0112_1 19:967207de919d 49
lilac0112_1 24:34ef6379b0df 50 //初期値設定の終了
lilac0112_1 10:6df631c39f9b 51 sys.KickOffFlag=0;
lilac0112_1 0:ea35c18c85fc 52 }
lilac0112_1 24:34ef6379b0df 53 ////DataRetrieve
lilac0112_1 19:967207de919d 54 if(sys.InfoFlag==1){ReadInfo();sys.InfoFlag=0;}
lilac0112_1 11:3efae754e6ef 55 data.lnRaw = LineRaw;
lilac0112_1 11:3efae754e6ef 56 data.lnHold = LineHold;
lilac0112_1 21:378470320524 57 data.ball = ReadBall();
lilac0112_1 19:967207de919d 58
lilac0112_1 24:34ef6379b0df 59 //ボールがなければ自分のゴールに戻る
lilac0112_1 24:34ef6379b0df 60 if(data.irNotice==IR_NONE){
lilac0112_1 24:34ef6379b0df 61 sys.BackHomeFlag=(sys.HomeBlind==0);
lilac0112_1 24:34ef6379b0df 62 }
lilac0112_1 24:34ef6379b0df 63 else{
lilac0112_1 24:34ef6379b0df 64 sys.BackHomeFlag=0;
lilac0112_1 25:a7460e23e02e 65 sys.HomeStayFlag[X_PING]=0;
lilac0112_1 25:a7460e23e02e 66 sys.HomeStayFlag[Y_PING]=0;
lilac0112_1 24:34ef6379b0df 67 }
lilac0112_1 24:34ef6379b0df 68 //回り込みの値を代入
lilac0112_1 24:34ef6379b0df 69 if(data.ping[B_PING]<=30){
lilac0112_1 24:34ef6379b0df 70 sys.ir_pow_table=1;
lilac0112_1 24:34ef6379b0df 71 ir_x_dir = ir_move_val[1][data.irNotice][data.irPosition][IR_X_DIR];
lilac0112_1 24:34ef6379b0df 72 ir_y_dir = ir_move_val[1][data.irNotice][data.irPosition][IR_Y_DIR];
lilac0112_1 24:34ef6379b0df 73 ir_x_turn = ir_move_val[1][data.irNotice][data.irPosition][IR_X_TURN];
lilac0112_1 24:34ef6379b0df 74 ir_y_turn = ir_move_val[1][data.irNotice][data.irPosition][IR_Y_TURN];
lilac0112_1 24:34ef6379b0df 75 }
lilac0112_1 24:34ef6379b0df 76 else{
lilac0112_1 24:34ef6379b0df 77 sys.ir_pow_table=0;
lilac0112_1 24:34ef6379b0df 78 ir_x_dir = ir_move_val[0][data.irNotice][data.irPosition][IR_X_DIR];
lilac0112_1 24:34ef6379b0df 79 ir_y_dir = ir_move_val[0][data.irNotice][data.irPosition][IR_Y_DIR];
lilac0112_1 24:34ef6379b0df 80 ir_x_turn = ir_move_val[0][data.irNotice][data.irPosition][IR_X_TURN];
lilac0112_1 24:34ef6379b0df 81 ir_y_turn = ir_move_val[0][data.irNotice][data.irPosition][IR_Y_TURN];
lilac0112_1 24:34ef6379b0df 82 }
lilac0112_1 24:34ef6379b0df 83 //Irの検出値によって出力を調整
lilac0112_1 24:34ef6379b0df 84 if(data.irNotice==IR_CLOSER){
lilac0112_1 25:a7460e23e02e 85 //ir_pow = sys.s_pow;
lilac0112_1 25:a7460e23e02e 86 ir_pow_x = ir_pow_y = sys.s_pow;
lilac0112_1 24:34ef6379b0df 87 }
lilac0112_1 24:34ef6379b0df 88 else if(data.irNotice==IR_CLOSE){
lilac0112_1 25:a7460e23e02e 89 //ir_pow = sys.m_pow;
lilac0112_1 25:a7460e23e02e 90 ir_pow_x = ir_pow_y = sys.m_pow;
lilac0112_1 24:34ef6379b0df 91 }
lilac0112_1 24:34ef6379b0df 92 else if(data.irNotice==IR_FAR){
lilac0112_1 25:a7460e23e02e 93 //ir_pow = sys.l_pow;
lilac0112_1 25:a7460e23e02e 94 ir_pow_x = ir_pow_y = sys.l_pow;
lilac0112_1 24:34ef6379b0df 95 }
lilac0112_1 24:34ef6379b0df 96 else{//data.irNotice==IR_NONE
lilac0112_1 25:a7460e23e02e 97 //ir_pow = 0;
lilac0112_1 25:a7460e23e02e 98 ir_pow_x = ir_pow_y = 0;
lilac0112_1 24:34ef6379b0df 99 }
lilac0112_1 24:34ef6379b0df 100 ////ドリブラー
lilac0112_1 24:34ef6379b0df 101 //ホールド判定
lilac0112_1 25:a7460e23e02e 102 JudgeBallHolding();
lilac0112_1 25:a7460e23e02e 103 //ドリブラー駆動
lilac0112_1 25:a7460e23e02e 104 if((sys.DriBlind==0)&&(data.irNotice==IR_CLOSER)&&((data.irPosition==10)||(data.irPosition==11)||(data.irPosition==12))){
lilac0112_1 25:a7460e23e02e 105 sys.DribbleFlag=1;
lilac0112_1 25:a7460e23e02e 106 if(sys.BallHoldFlag==1){
lilac0112_1 25:a7460e23e02e 107 //ir_pow=20;
lilac0112_1 25:a7460e23e02e 108 ir_pow_x = ir_pow_y = 20;
lilac0112_1 19:967207de919d 109 }
lilac0112_1 19:967207de919d 110 }
lilac0112_1 19:967207de919d 111 else{
lilac0112_1 19:967207de919d 112 sys.DribbleFlag=0;
lilac0112_1 25:a7460e23e02e 113 cmps_set.HoldDeg=0;
lilac0112_1 19:967207de919d 114 }
lilac0112_1 19:967207de919d 115
lilac0112_1 24:34ef6379b0df 116 ////Kick
lilac0112_1 25:a7460e23e02e 117 /*if((data.ball==1)&&(data.irNotice==IR_CLOSER)&&((data.irPosition==10)||(data.irPosition==11)||(data.irPosition==12))){
lilac0112_1 24:34ef6379b0df 118 DriveTurn();
lilac0112_1 25:a7460e23e02e 119 }*/
lilac0112_1 25:a7460e23e02e 120 /*if((data.ball==1)&&(data.irNotice==IR_CLOSER)&&((data.irPosition==10)||(data.irPosition==11)||(data.irPosition==12))){
lilac0112_1 25:a7460e23e02e 121 DriveTurn();
lilac0112_1 25:a7460e23e02e 122 DriveSolenoid();
lilac0112_1 25:a7460e23e02e 123 }*/
lilac0112_1 19:967207de919d 124
lilac0112_1 24:34ef6379b0df 125 //Irの細かい補正値の処理．基本的にLineの補正値を演算する処理が始まる前にIrのモーター出力値補正をすべて終わらせる．
lilac0112_1 25:a7460e23e02e 126 //if(sys.IrBlind==1) ir_pow=0;
lilac0112_1 25:a7460e23e02e 127 if(sys.IrBlind==1) ir_pow_x=ir_pow_y=0;
lilac0112_1 18:3a42a931c95a 128 if((data.ping[R_PING]<data.ping[L_PING])&&(data.irPosition==17)){
lilac0112_1 18:3a42a931c95a 129 ir_x_turn = -ir_x_turn;
lilac0112_1 18:3a42a931c95a 130 ir_y_turn = -ir_y_turn;
lilac0112_1 18:3a42a931c95a 131 }
lilac0112_1 25:a7460e23e02e 132
lilac0112_1 26:6ca88eeaa2b4 133 if((sys.BackHomeFlag==1)&&(sys.HomeBlind==0)){
lilac0112_1 25:a7460e23e02e 134 ir_pow_x = ir_pow_y = 25;
lilac0112_1 25:a7460e23e02e 135 //x
lilac0112_1 25:a7460e23e02e 136 if((abs(data.ping[L_PING]-data.ping[R_PING])>20)&&(sys.HomeStayFlag[X_PING]==0)){
lilac0112_1 25:a7460e23e02e 137 if(data.ping[L_PING]>data.ping[R_PING]){
lilac0112_1 25:a7460e23e02e 138 ir_x = -1;
lilac0112_1 25:a7460e23e02e 139 }
lilac0112_1 25:a7460e23e02e 140 else{
lilac0112_1 25:a7460e23e02e 141 ir_x = 1;
lilac0112_1 25:a7460e23e02e 142 }
lilac0112_1 25:a7460e23e02e 143 if((data.ping[L_PING]<WhiteToWallPlus[X_PING])&&(data.ping[R_PING]<WhiteToWallPlus[X_PING])){
lilac0112_1 25:a7460e23e02e 144 ir_x = 0;
lilac0112_1 25:a7460e23e02e 145 }
lilac0112_1 25:a7460e23e02e 146 }
lilac0112_1 25:a7460e23e02e 147 else{
lilac0112_1 25:a7460e23e02e 148 ir_x = 0;
lilac0112_1 25:a7460e23e02e 149 //sys.HomeStayFlag[X_PING]=1;
lilac0112_1 25:a7460e23e02e 150 }
lilac0112_1 25:a7460e23e02e 151 //y
lilac0112_1 25:a7460e23e02e 152 if((data.ping[B_PING]>40)&&(1)&&(sys.HomeStayFlag[Y_PING]==0)){
lilac0112_1 25:a7460e23e02e 153 ir_y = -1;
lilac0112_1 25:a7460e23e02e 154 }
lilac0112_1 25:a7460e23e02e 155 else{
lilac0112_1 25:a7460e23e02e 156 ir_y = 0;
lilac0112_1 25:a7460e23e02e 157 //sys.HomeStayFlag[Y_PING]=1;
lilac0112_1 25:a7460e23e02e 158 }
lilac0112_1 25:a7460e23e02e 159 }
lilac0112_1 25:a7460e23e02e 160 else{
lilac0112_1 25:a7460e23e02e 161 ir_x = (ir_x_dir + ir_x_turn);
lilac0112_1 25:a7460e23e02e 162 ir_y = (ir_y_dir + ir_y_turn);
lilac0112_1 25:a7460e23e02e 163 }
lilac0112_1 25:a7460e23e02e 164
lilac0112_1 25:a7460e23e02e 165 ////超音波による減速
lilac0112_1 25:a7460e23e02e 166 /*if((data.irNotice==IR_CLOSE)||(data.irNotice==IR_CLOSER)){
lilac0112_1 25:a7460e23e02e 167 if(
lilac0112_1 25:a7460e23e02e 168 (
lilac0112_1 25:a7460e23e02e 169 (data.ping[L_PING]<WhiteToWallPlus[X_PING])&&
lilac0112_1 25:a7460e23e02e 170 (data.ping[R_PING]>WhiteToWallPlus[X_PING])&&
lilac0112_1 25:a7460e23e02e 171 (ir_x>0)
lilac0112_1 25:a7460e23e02e 172 )||
lilac0112_1 25:a7460e23e02e 173 (
lilac0112_1 25:a7460e23e02e 174 (data.ping[L_PING]>WhiteToWallPlus[X_PING])&&
lilac0112_1 25:a7460e23e02e 175 (data.ping[R_PING]<WhiteToWallPlus[X_PING])&&
lilac0112_1 25:a7460e23e02e 176 (ir_x<0)
lilac0112_1 25:a7460e23e02e 177 )
lilac0112_1 25:a7460e23e02e 178 ){
lilac0112_1 25:a7460e23e02e 179
lilac0112_1 25:a7460e23e02e 180 }
lilac0112_1 25:a7460e23e02e 181 }*/
lilac0112_1 24:34ef6379b0df 182 ////Lineセンサーの値を評価しモーターの出力補正値を演算
lilac0112_1 14:b510adcb6065 183 if(sys.LineBlind==1){
lilac0112_1 14:b510adcb6065 184
lilac0112_1 14:b510adcb6065 185 LineSlowPower[X_LINE] = 1.0;
lilac0112_1 14:b510adcb6065 186 LineSlowPower[Y_LINE] = 1.0;
lilac0112_1 14:b510adcb6065 187
lilac0112_1 14:b510adcb6065 188 LineReturnPower[X_LINE] = 0.0;
lilac0112_1 14:b510adcb6065 189 LineReturnPower[Y_LINE] = 0.0;
lilac0112_1 14:b510adcb6065 190
lilac0112_1 14:b510adcb6065 191 data.lnStop[X_LINE] = 1;
lilac0112_1 14:b510adcb6065 192 data.lnStop[Y_LINE] = 1;
lilac0112_1 14:b510adcb6065 193
lilac0112_1 15:17502a27a60b 194 data.FieldSpot = LINE_INSIDE;
lilac0112_1 14:b510adcb6065 195 }
lilac0112_1 14:b510adcb6065 196 else{
lilac0112_1 18:3a42a931c95a 197 LineJudgeReset(ir_x, ir_y, &LineSlowPower[X_LINE], &LineSlowPower[Y_LINE]);
lilac0112_1 18:3a42a931c95a 198 LineJudgeSlow(ir_x, ir_y, &LineSlowPower[X_LINE], &LineSlowPower[Y_LINE]);
lilac0112_1 18:3a42a931c95a 199 LineJudgeReturn(ir_x*LineSlowPower[X_LINE], ir_y*LineSlowPower[Y_LINE], &LineReturnPower[X_LINE], &LineReturnPower[Y_LINE]);
lilac0112_1 14:b510adcb6065 200 }
lilac0112_1 24:34ef6379b0df 201 ////LEDデバッグ
lilac0112_1 19:967207de919d 202 //if(data.FieldSpot==LINE_OUTSIDE) LED = 0x9;
lilac0112_1 19:967207de919d 203 //if(data.FieldSpot==LINE_INSIDE) LED = 0x6;
lilac0112_1 19:967207de919d 204
lilac0112_1 25:a7460e23e02e 205 //LED = sys.BallHoldFlag*15;
lilac0112_1 25:a7460e23e02e 206 if(sys.BallHoldFlag==1) LED=15;
lilac0112_1 25:a7460e23e02e 207 else LED=10;
lilac0112_1 19:967207de919d 208
lilac0112_1 21:378470320524 209 //LED = 0xFF*(data.ping[B_PING]<30);
lilac0112_1 14:b510adcb6065 210
lilac0112_1 16:4fadb7a87497 211 //LED = ((data.lnOrder[0]!=LINE_EMPTY)<<2) | ((data.lnOrder[1]!=LINE_EMPTY)<<1) | ((data.lnOrder[2]!=LINE_EMPTY)<<0);
lilac0112_1 14:b510adcb6065 212
lilac0112_1 14:b510adcb6065 213 //else LED = 0xA;
lilac0112_1 14:b510adcb6065 214 //LED = LineHold;
lilac0112_1 14:b510adcb6065 215
lilac0112_1 24:34ef6379b0df 216
lilac0112_1 24:34ef6379b0df 217 ////最終的なモーターの出力を演算
lilac0112_1 25:a7460e23e02e 218 vx = (ir_pow_x*ir_x)*data.lnStop[X_LINE]*(data.lnStay[X_LINE])*LineSlowPower[X_LINE] + LineReturnPower[X_LINE];
lilac0112_1 25:a7460e23e02e 219 vy = (ir_pow_y*ir_y)*data.lnStop[Y_LINE]*(data.lnStay[Y_LINE])*LineSlowPower[Y_LINE] + LineReturnPower[Y_LINE];
lilac0112_1 10:6df631c39f9b 220 vs = cmps_set.OutputPID;
lilac0112_1 10:6df631c39f9b 221 move(
lilac0112_1 10:6df631c39f9b 222 vx,
lilac0112_1 10:6df631c39f9b 223 vy,
lilac0112_1 10:6df631c39f9b 224 vs
lilac0112_1 10:6df631c39f9b 225 );
lilac0112_1 24:34ef6379b0df 226 //モーターに信号を出力
lilac0112_1 10:6df631c39f9b 227 if(sys.MotorFlag==1){tx_motor();sys.MotorFlag=0;}
lilac0112_1 24:34ef6379b0df 228
lilac0112_1 0:ea35c18c85fc 229 if(sys.stopflag==1){
lilac0112_1 24:34ef6379b0df 230 //コマンドモードに戻る際の処理
lilac0112_1 0:ea35c18c85fc 231 }
lilac0112_1 0:ea35c18c85fc 232 return;
lilac0112_1 0:ea35c18c85fc 233 }
lilac0112_1 0:ea35c18c85fc 234 void modeAttack5(void){
lilac0112_1 21:378470320524 235 /*if(sys.IrFlag==1){
lilac0112_1 0:ea35c18c85fc 236 ReadIr();
lilac0112_1 0:ea35c18c85fc 237 sys.IrFlag=0;
lilac0112_1 0:ea35c18c85fc 238 }
lilac0112_1 0:ea35c18c85fc 239 if(sys.PidFlag==1){
lilac0112_1 0:ea35c18c85fc 240 PidUpdate();
lilac0112_1 0:ea35c18c85fc 241 sys.PidFlag=0;
lilac0112_1 21:378470320524 242 }*/
lilac0112_1 10:6df631c39f9b 243 move(0,0,cmps_set.OutputPID);
lilac0112_1 0:ea35c18c85fc 244 if(sys.MotorFlag==1){
lilac0112_1 14:b510adcb6065 245 //LED[0] = 1;
lilac0112_1 14:b510adcb6065 246 //LED[1] = 0;
lilac0112_1 0:ea35c18c85fc 247 tx_motor();
lilac0112_1 0:ea35c18c85fc 248 sys.MotorFlag=0;
lilac0112_1 0:ea35c18c85fc 249 }
lilac0112_1 5:5ff3a7d5d8c2 250 else{
lilac0112_1 14:b510adcb6065 251 //LED[0] = 0;
lilac0112_1 14:b510adcb6065 252 //LED[1] = 1;
lilac0112_1 5:5ff3a7d5d8c2 253 }
lilac0112_1 0:ea35c18c85fc 254 if(sys.stopflag==1){
lilac0112_1 0:ea35c18c85fc 255 //停止処理
lilac0112_1 0:ea35c18c85fc 256 }
lilac0112_1 0:ea35c18c85fc 257 return;
lilac0112_1 0:ea35c18c85fc 258 }