CatPot 2015-2016 / Mbed 2 deprecated CatPot_2v10_T_Main

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed AQM1602 HMC6352 PID

main_processing/strategy/strategy.cpp@27:825c6835e3db, 2016-03-21 (annotated)

Committer:
lilac0112_1
Date:
Mon Mar 21 17:05:08 2016 +0000
Revision:
27:825c6835e3db
Parent:
26:6ca88eeaa2b4
Child:
28:f246a5fbf6a5
somehow

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