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@5:5ff3a7d5d8c2, 2016-03-04 (annotated)

Committer:
lilac0112_1
Date:
Fri Mar 04 09:01:20 2016 +0000
Revision:
5:5ff3a7d5d8c2
Parent:
2:635947de1583
Child:
10:6df631c39f9b
somehow and new ir_pow_format

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 modeAttack0(void){
lilac0112_1 0:ea35c18c85fc 6 double ir_x, ir_y;
lilac0112_1 0:ea35c18c85fc 7 int vx,vy,vs;
lilac0112_1 0:ea35c18c85fc 8 uint8_t LineDir[4];
lilac0112_1 0:ea35c18c85fc 9 uint8_t LineStop[2];
lilac0112_1 0:ea35c18c85fc 10 //uint8_t IrRange[4];
lilac0112_1 0:ea35c18c85fc 11 //uint8_t LineBind[4];
lilac0112_1 0:ea35c18c85fc 12 if(sys.IrFlag==1){
lilac0112_1 0:ea35c18c85fc 13 ReadIr();
lilac0112_1 0:ea35c18c85fc 14 sys.IrFlag=0;
lilac0112_1 0:ea35c18c85fc 15 }
lilac0112_1 0:ea35c18c85fc 16 if(sys.PidFlag==1){
lilac0112_1 0:ea35c18c85fc 17 Line_ticker.detach();
lilac0112_1 0:ea35c18c85fc 18 PidUpdate();
lilac0112_1 0:ea35c18c85fc 19 Line_ticker.attach(&ReadLine, 0.005);
lilac0112_1 0:ea35c18c85fc 20 sys.PidFlag=0;
lilac0112_1 0:ea35c18c85fc 21 }
lilac0112_1 5:5ff3a7d5d8c2 22 ir_x = ir_move_val_old[data.irNotice][data.irPosition][IR_X];
lilac0112_1 5:5ff3a7d5d8c2 23 ir_y = ir_move_val_old[data.irNotice][data.irPosition][IR_Y];
lilac0112_1 2:635947de1583 24 if(data.irPosition<8){
lilac0112_1 0:ea35c18c85fc 25 ir_x *= (double)(sys.l_pow);
lilac0112_1 0:ea35c18c85fc 26 ir_y *= (double)(sys.l_pow);
lilac0112_1 0:ea35c18c85fc 27 }
lilac0112_1 0:ea35c18c85fc 28 else{
lilac0112_1 0:ea35c18c85fc 29 ir_x *= (double)(sys.s_pow);
lilac0112_1 0:ea35c18c85fc 30 ir_y *= (double)(sys.s_pow);
lilac0112_1 0:ea35c18c85fc 31 }
lilac0112_1 0:ea35c18c85fc 32 //Lineを考慮していないIrのみの値
lilac0112_1 0:ea35c18c85fc 33 vx = ir_x;
lilac0112_1 0:ea35c18c85fc 34 vy = ir_y;
lilac0112_1 0:ea35c18c85fc 35 //Line検出方向を調べる
lilac0112_1 2:635947de1583 36 LineDir[A_SPOT] = (!((vx>0)&&((data.lnFlag[A_SPOT]==1)&&(data.lnFlag[B_SPOT]==0))));
lilac0112_1 2:635947de1583 37 LineDir[B_SPOT] = (!((vx<0)&&((data.lnFlag[A_SPOT]==0)&&(data.lnFlag[B_SPOT]==1))));
lilac0112_1 2:635947de1583 38 LineDir[C_SPOT] = (!((vy<0)&&((data.lnFlag[C_SPOT]==1)&&(1))));
lilac0112_1 2:635947de1583 39 LineDir[AB_SPOT] = (!((vy>0)&&((data.lnFlag[A_SPOT]==1)&&(data.lnFlag[B_SPOT]==1))));
lilac0112_1 0:ea35c18c85fc 40
lilac0112_1 0:ea35c18c85fc 41 LineStop[X_AXIS] = LineDir[A_SPOT]*LineDir[B_SPOT];
lilac0112_1 0:ea35c18c85fc 42 LineStop[Y_AXIS] = LineDir[C_SPOT]*LineDir[AB_SPOT];
lilac0112_1 0:ea35c18c85fc 43
lilac0112_1 0:ea35c18c85fc 44 //Ir
lilac0112_1 0:ea35c18c85fc 45 move(
lilac0112_1 0:ea35c18c85fc 46 vx*LineStop[X_AXIS] + (LINE_RF)*(vy!=0)*((-1)*(LineDir[A_SPOT]==0) + (LineDir[B_SPOT]==0)),
lilac0112_1 0:ea35c18c85fc 47 vy*LineStop[Y_AXIS] + (LINE_RF)*(vx!=0)*((LineDir[C_SPOT]==0) + (-1)*(LineDir[AB_SPOT]==0)),
lilac0112_1 0:ea35c18c85fc 48 vs
lilac0112_1 0:ea35c18c85fc 49 );
lilac0112_1 0:ea35c18c85fc 50 if(sys.MotorFlag==1){
lilac0112_1 0:ea35c18c85fc 51 tx_motor();
lilac0112_1 0:ea35c18c85fc 52 sys.MotorFlag=0;
lilac0112_1 0:ea35c18c85fc 53 }
lilac0112_1 0:ea35c18c85fc 54 return;
lilac0112_1 0:ea35c18c85fc 55 }
lilac0112_1 0:ea35c18c85fc 56 uint8_t HmcResetFlag;
lilac0112_1 0:ea35c18c85fc 57 void HmcReset(void){
lilac0112_1 0:ea35c18c85fc 58 hmc_reset=1;
lilac0112_1 0:ea35c18c85fc 59 wait_us(100);
lilac0112_1 0:ea35c18c85fc 60 hmc_reset=0;
lilac0112_1 0:ea35c18c85fc 61 }
lilac0112_1 0:ea35c18c85fc 62 uint8_t PingFlag;
lilac0112_1 0:ea35c18c85fc 63 void ValidPing(void){
lilac0112_1 0:ea35c18c85fc 64 if(PingFlag==0){
lilac0112_1 0:ea35c18c85fc 65 PingFlag=1;
lilac0112_1 0:ea35c18c85fc 66 }
lilac0112_1 0:ea35c18c85fc 67 }
lilac0112_1 0:ea35c18c85fc 68 uint8_t LineReverseFlag;
lilac0112_1 0:ea35c18c85fc 69 void LineReverse(void){
lilac0112_1 0:ea35c18c85fc 70 LineReverseFlag=0;
lilac0112_1 0:ea35c18c85fc 71 }
lilac0112_1 0:ea35c18c85fc 72 uint8_t LineSign[3];
lilac0112_1 0:ea35c18c85fc 73 uint8_t LineFirst[2];
lilac0112_1 0:ea35c18c85fc 74 uint8_t LinePriority[2];//0を後に，1を優先
lilac0112_1 2:635947de1583 75 void LineClear_A(void){LineSign[A_SPOT]=0;data.lnFlag[A_SPOT]=0;}
lilac0112_1 2:635947de1583 76 void LineClear_B(void){LineSign[B_SPOT]=0;data.lnFlag[B_SPOT]=0;}
lilac0112_1 2:635947de1583 77 void LineClear_C(void){LineSign[C_SPOT]=0;data.lnFlag[C_SPOT]=0;}
lilac0112_1 0:ea35c18c85fc 78 void LineCall_A(void){
lilac0112_1 0:ea35c18c85fc 79 //
lilac0112_1 0:ea35c18c85fc 80 LineSign[A_SPOT] = 1;
lilac0112_1 0:ea35c18c85fc 81 if(LineSign[B_SPOT]==0){
lilac0112_1 0:ea35c18c85fc 82 LineFirst[X_AXIS] = A_SPOT;
lilac0112_1 0:ea35c18c85fc 83 }
lilac0112_1 0:ea35c18c85fc 84 if((LineSign[A_SPOT]==1)&&(LineSign[B_SPOT]==1)){
lilac0112_1 0:ea35c18c85fc 85 if(LineSign[C_SPOT]==0){
lilac0112_1 0:ea35c18c85fc 86 LineFirst[Y_AXIS] = AB_SPOT;
lilac0112_1 0:ea35c18c85fc 87 }
lilac0112_1 0:ea35c18c85fc 88 }
lilac0112_1 0:ea35c18c85fc 89 //
lilac0112_1 2:635947de1583 90 if((Line[A_SPOT].read()==1)||(1)) data.lnFlag[A_SPOT]=1;
lilac0112_1 0:ea35c18c85fc 91 //
lilac0112_1 0:ea35c18c85fc 92 Line_timeout[A_SPOT].attach(&LineClear_A, LINE_DELAY);
lilac0112_1 0:ea35c18c85fc 93 }
lilac0112_1 0:ea35c18c85fc 94 void LineCall_B(void){
lilac0112_1 0:ea35c18c85fc 95 //
lilac0112_1 0:ea35c18c85fc 96 LineSign[B_SPOT] = 1;
lilac0112_1 0:ea35c18c85fc 97 if(LineSign[A_SPOT]==0){
lilac0112_1 0:ea35c18c85fc 98 LineFirst[X_AXIS] = B_SPOT;
lilac0112_1 0:ea35c18c85fc 99 }
lilac0112_1 0:ea35c18c85fc 100 if((LineSign[A_SPOT]==1)&&(LineSign[B_SPOT]==1)){
lilac0112_1 0:ea35c18c85fc 101 if(LineSign[C_SPOT]==0){
lilac0112_1 0:ea35c18c85fc 102 LineFirst[Y_AXIS] = AB_SPOT;
lilac0112_1 0:ea35c18c85fc 103 }
lilac0112_1 0:ea35c18c85fc 104 }
lilac0112_1 0:ea35c18c85fc 105 //
lilac0112_1 2:635947de1583 106 if((Line[B_SPOT].read()==1)||(1)) data.lnFlag[B_SPOT]=1;
lilac0112_1 0:ea35c18c85fc 107 //
lilac0112_1 0:ea35c18c85fc 108 Line_timeout[B_SPOT].attach(&LineClear_B, LINE_DELAY);
lilac0112_1 0:ea35c18c85fc 109 }
lilac0112_1 0:ea35c18c85fc 110 void LineCall_C(void){
lilac0112_1 0:ea35c18c85fc 111 //
lilac0112_1 0:ea35c18c85fc 112 LineSign[C_SPOT] = 1;
lilac0112_1 0:ea35c18c85fc 113 if(!((LineSign[A_SPOT]==1)&&(LineSign[B_SPOT]==1))){
lilac0112_1 0:ea35c18c85fc 114 LineFirst[Y_AXIS] = C_SPOT;
lilac0112_1 0:ea35c18c85fc 115 }
lilac0112_1 0:ea35c18c85fc 116 //
lilac0112_1 2:635947de1583 117 if((Line[C_SPOT].read()==1)||(1)) data.lnFlag[C_SPOT]=1;
lilac0112_1 0:ea35c18c85fc 118 //
lilac0112_1 0:ea35c18c85fc 119 Line_timeout[C_SPOT].attach(&LineClear_C, LINE_DELAY);
lilac0112_1 0:ea35c18c85fc 120 }
lilac0112_1 0:ea35c18c85fc 121 void modeAttack1(void){
lilac0112_1 0:ea35c18c85fc 122 double ir_x, ir_y;
lilac0112_1 0:ea35c18c85fc 123 int vx,vy,vs;
lilac0112_1 0:ea35c18c85fc 124 uint8_t LineDir[4];
lilac0112_1 0:ea35c18c85fc 125 uint8_t LineStop[2];
lilac0112_1 0:ea35c18c85fc 126 uint8_t IrRange[4];
lilac0112_1 0:ea35c18c85fc 127 uint8_t LinePulse[4];
lilac0112_1 0:ea35c18c85fc 128 uint8_t static LineBind[4];
lilac0112_1 0:ea35c18c85fc 129 if(sys.KickOffFlag==1){
lilac0112_1 0:ea35c18c85fc 130 LineBind[0]=0;
lilac0112_1 0:ea35c18c85fc 131 LineBind[1]=0;
lilac0112_1 0:ea35c18c85fc 132 LineBind[2]=0;
lilac0112_1 0:ea35c18c85fc 133 LineBind[3]=0;
lilac0112_1 0:ea35c18c85fc 134 LineReverseFlag=0;
lilac0112_1 0:ea35c18c85fc 135
lilac0112_1 0:ea35c18c85fc 136 LineSign[A_SPOT]=0;
lilac0112_1 0:ea35c18c85fc 137 LineSign[B_SPOT]=0;
lilac0112_1 0:ea35c18c85fc 138 LineSign[C_SPOT]=0;
lilac0112_1 0:ea35c18c85fc 139
lilac0112_1 2:635947de1583 140 data.lnFlag[A_SPOT]=0;
lilac0112_1 2:635947de1583 141 data.lnFlag[B_SPOT]=0;
lilac0112_1 2:635947de1583 142 data.lnFlag[C_SPOT]=0;
lilac0112_1 0:ea35c18c85fc 143
lilac0112_1 0:ea35c18c85fc 144 sys.KickOffFlag=0;
lilac0112_1 0:ea35c18c85fc 145 }
lilac0112_1 0:ea35c18c85fc 146 if(sys.IrFlag==1){
lilac0112_1 0:ea35c18c85fc 147 ReadIr();
lilac0112_1 0:ea35c18c85fc 148 sys.IrFlag=0;
lilac0112_1 0:ea35c18c85fc 149 }
lilac0112_1 0:ea35c18c85fc 150 if(sys.PidFlag==1){
lilac0112_1 0:ea35c18c85fc 151 //Line_ticker.detach();
lilac0112_1 0:ea35c18c85fc 152 PidUpdate();
lilac0112_1 0:ea35c18c85fc 153 //Line_ticker.attach(&ReadLine, 0.005);
lilac0112_1 0:ea35c18c85fc 154 sys.PidFlag=0;
lilac0112_1 0:ea35c18c85fc 155 }
lilac0112_1 5:5ff3a7d5d8c2 156 ir_x = ir_move_val_old[data.irNotice][data.irPosition][IR_X];
lilac0112_1 5:5ff3a7d5d8c2 157 ir_y = ir_move_val_old[data.irNotice][data.irPosition][IR_Y];
lilac0112_1 2:635947de1583 158 if(data.irPosition<8){
lilac0112_1 0:ea35c18c85fc 159 ir_x *= sys.l_pow;
lilac0112_1 0:ea35c18c85fc 160 ir_y *= sys.l_pow;
lilac0112_1 0:ea35c18c85fc 161 }
lilac0112_1 0:ea35c18c85fc 162 else{
lilac0112_1 0:ea35c18c85fc 163 ir_x *= sys.s_pow;
lilac0112_1 0:ea35c18c85fc 164 ir_y *= sys.s_pow;
lilac0112_1 0:ea35c18c85fc 165 }
lilac0112_1 0:ea35c18c85fc 166
lilac0112_1 0:ea35c18c85fc 167 //Lineを考慮していないIrのみの値
lilac0112_1 0:ea35c18c85fc 168 vx = ir_x;
lilac0112_1 0:ea35c18c85fc 169 vy = ir_y;
lilac0112_1 0:ea35c18c85fc 170 //Line検出方向を調べる
lilac0112_1 2:635947de1583 171 LineDir[A_SPOT] = (!((vx>0)&&((data.lnFlag[A_SPOT]==1)&&(data.lnFlag[B_SPOT]==0))&&(1)));
lilac0112_1 2:635947de1583 172 LineDir[B_SPOT] = (!((vx<0)&&((data.lnFlag[A_SPOT]==0)&&(data.lnFlag[B_SPOT]==1))&&(1)));
lilac0112_1 2:635947de1583 173 LineDir[C_SPOT] = (!((vy<0)&&((data.lnFlag[C_SPOT]==1)&&(1 ))&&(1)));
lilac0112_1 2:635947de1583 174 LineDir[AB_SPOT] = (!((vy>0)&&((data.lnFlag[A_SPOT]==1)&&(data.lnFlag[B_SPOT]==1))&&(1)));
lilac0112_1 0:ea35c18c85fc 175
lilac0112_1 0:ea35c18c85fc 176 LineStop[X_AXIS] = LineDir[A_SPOT]*LineDir[B_SPOT];
lilac0112_1 0:ea35c18c85fc 177 LineStop[Y_AXIS] = LineDir[C_SPOT]*LineDir[AB_SPOT];
lilac0112_1 0:ea35c18c85fc 178
lilac0112_1 0:ea35c18c85fc 179 //Ir
lilac0112_1 0:ea35c18c85fc 180 //strict
lilac0112_1 0:ea35c18c85fc 181 /*
lilac0112_1 2:635947de1583 182 IrRange[A_SPOT] = ((18<=data.irPosition)&&(data.irPosition<=19))||((8<=data.irPosition)&&(data.irPosition<=10))
lilac0112_1 2:635947de1583 183 ||((6<=data.irPosition)&&(data.irPosition<=7))||(( 1)&&(data.irPosition<=1));
lilac0112_1 2:635947de1583 184 IrRange[B_SPOT] = ((12<=data.irPosition)&&(data.irPosition<=16))||((2<=data.irPosition)&&(data.irPosition<=5));
lilac0112_1 2:635947de1583 185 IrRange[C_SPOT] = ((15<=data.irPosition)&&(data.irPosition<=19))||((4<=data.irPosition)&&(data.irPosition<=7));
lilac0112_1 2:635947de1583 186 IrRange[AB_SPOT] = ((9<=data.irPosition)&&(data.irPosition<=13))||(( 1)&&(data.irPosition<=3));
lilac0112_1 0:ea35c18c85fc 187 */
lilac0112_1 0:ea35c18c85fc 188 //sweet
lilac0112_1 2:635947de1583 189 IrRange[A_SPOT] = ((19<=data.irPosition)&&(data.irPosition<=19))||((8<=data.irPosition)&&(data.irPosition<=9))
lilac0112_1 2:635947de1583 190 ||((6<=data.irPosition)&&(data.irPosition<=7))||(( 1)&&(data.irPosition<=1));
lilac0112_1 2:635947de1583 191 IrRange[B_SPOT] = ((13<=data.irPosition)&&(data.irPosition<=15))||((2<=data.irPosition)&&(data.irPosition<=5));
lilac0112_1 2:635947de1583 192 IrRange[C_SPOT] = ((16<=data.irPosition)&&(data.irPosition<=18))||((4<=data.irPosition)&&(data.irPosition<=7));
lilac0112_1 2:635947de1583 193 IrRange[AB_SPOT] = ((10<=data.irPosition)&&(data.irPosition<=12))||(( 1)&&(data.irPosition<=3));
lilac0112_1 0:ea35c18c85fc 194
lilac0112_1 0:ea35c18c85fc 195
lilac0112_1 0:ea35c18c85fc 196 LinePulse[A_SPOT] = ((IrRange[A_SPOT]==1)&&(LineDir[A_SPOT]==0));
lilac0112_1 0:ea35c18c85fc 197 LinePulse[B_SPOT] = ((IrRange[B_SPOT]==1)&&(LineDir[B_SPOT]==0));
lilac0112_1 0:ea35c18c85fc 198 LinePulse[C_SPOT] = ((IrRange[C_SPOT]==1)&&(LineDir[C_SPOT]==0));
lilac0112_1 0:ea35c18c85fc 199 LinePulse[AB_SPOT] = ((IrRange[AB_SPOT]==1)&&(LineDir[AB_SPOT]==0));
lilac0112_1 0:ea35c18c85fc 200
lilac0112_1 0:ea35c18c85fc 201 LineBind[A_SPOT] = ((LinePulse[A_SPOT])||((IrRange[A_SPOT]==1)&&(LineBind[A_SPOT]==1)))&&(LineBind[B_SPOT]==0);
lilac0112_1 0:ea35c18c85fc 202 LineBind[B_SPOT] = ((LinePulse[B_SPOT])||((IrRange[B_SPOT]==1)&&(LineBind[B_SPOT]==1)))&&(LineBind[A_SPOT]==0);
lilac0112_1 0:ea35c18c85fc 203 LineBind[C_SPOT] = ((LinePulse[C_SPOT])||((IrRange[C_SPOT]==1)&&(LineBind[C_SPOT]==1)))&&(LineBind[AB_SPOT]==0);
lilac0112_1 0:ea35c18c85fc 204 LineBind[AB_SPOT] = ((LinePulse[AB_SPOT])||((IrRange[AB_SPOT]==1)&&(LineBind[AB_SPOT]==1)))&&(LineBind[C_SPOT]==0);
lilac0112_1 0:ea35c18c85fc 205 /*
lilac0112_1 0:ea35c18c85fc 206 LineBind[A_SPOT] = ((IrRange[A_SPOT]==1)&&((LineDir[A_SPOT]==0)||(LineBind[A_SPOT]==1)));
lilac0112_1 0:ea35c18c85fc 207 LineBind[B_SPOT] = ((IrRange[B_SPOT]==1)&&((LineDir[B_SPOT]==0)||(LineBind[B_SPOT]==1)));
lilac0112_1 0:ea35c18c85fc 208 LineBind[C_SPOT] = ((IrRange[C_SPOT]==1)&&((LineDir[C_SPOT]==0)||(LineBind[C_SPOT]==1)));
lilac0112_1 0:ea35c18c85fc 209 LineBind[AB_SPOT] = ((IrRange[AB_SPOT]==1)&&((LineDir[AB_SPOT]==0)||(LineBind[AB_SPOT]==1)));
lilac0112_1 0:ea35c18c85fc 210 */
lilac0112_1 0:ea35c18c85fc 211 vx = vx*LineStop[X_AXIS] + (LINE_RF)*(vy!=0)*((-1)*(LineDir[A_SPOT]==0) + (LineDir[B_SPOT]==0));
lilac0112_1 0:ea35c18c85fc 212 vy = vy*LineStop[Y_AXIS] + (LINE_RF)*(vx!=0)*((LineDir[C_SPOT]==0) + (-1)*(LineDir[AB_SPOT]==0));
lilac0112_1 0:ea35c18c85fc 213 vs = cmps_set.OutputPID;
lilac0112_1 0:ea35c18c85fc 214 if((LineBind[A_SPOT]==1)||(LineBind[B_SPOT]==1)||(LineBind[C_SPOT]==1)||(LineBind[AB_SPOT]==1)){
lilac0112_1 2:635947de1583 215 if(LineRaw>0){
lilac0112_1 0:ea35c18c85fc 216 vx=(LINE_RF*2)*((-1)*IrRange[A_SPOT] + IrRange[B_SPOT]);
lilac0112_1 0:ea35c18c85fc 217 vy=(LINE_RF*2)*( IrRange[C_SPOT] + (-1)*IrRange[AB_SPOT]);
lilac0112_1 0:ea35c18c85fc 218 /*vx=(LINE_RF*2)*((-1)*(LineFirst[X_AXIS] == A_SPOT) + (LineFirst[X_AXIS] == B_SPOT));
lilac0112_1 0:ea35c18c85fc 219 vy=(LINE_RF*2)*( (LineFirst[Y_AXIS] == C_SPOT) + (-1)*(LineFirst[Y_AXIS] == AB_SPOT));
lilac0112_1 0:ea35c18c85fc 220 Line_timeout[A_SPOT].attach(&LineClear_A, LINE_DELAY);
lilac0112_1 0:ea35c18c85fc 221 Line_timeout[B_SPOT].attach(&LineClear_B, LINE_DELAY);
lilac0112_1 0:ea35c18c85fc 222 Line_timeout[C_SPOT].attach(&LineClear_C, LINE_DELAY);*/
lilac0112_1 0:ea35c18c85fc 223 }
lilac0112_1 0:ea35c18c85fc 224 else{
lilac0112_1 0:ea35c18c85fc 225 vx=0;
lilac0112_1 0:ea35c18c85fc 226 vy=0;
lilac0112_1 0:ea35c18c85fc 227 }
lilac0112_1 0:ea35c18c85fc 228 }
lilac0112_1 2:635947de1583 229 if(LineRaw>0){
lilac0112_1 0:ea35c18c85fc 230 Line_timeout[A_SPOT].attach(&LineClear_A, LINE_DELAY);
lilac0112_1 0:ea35c18c85fc 231 Line_timeout[B_SPOT].attach(&LineClear_B, LINE_DELAY);
lilac0112_1 0:ea35c18c85fc 232 Line_timeout[C_SPOT].attach(&LineClear_C, LINE_DELAY);
lilac0112_1 0:ea35c18c85fc 233 }
lilac0112_1 0:ea35c18c85fc 234 move(
lilac0112_1 0:ea35c18c85fc 235 vx,
lilac0112_1 0:ea35c18c85fc 236 vy,
lilac0112_1 0:ea35c18c85fc 237 vs
lilac0112_1 0:ea35c18c85fc 238 );
lilac0112_1 0:ea35c18c85fc 239 if(sys.MotorFlag==1){
lilac0112_1 0:ea35c18c85fc 240 tx_motor();
lilac0112_1 0:ea35c18c85fc 241 sys.MotorFlag=0;
lilac0112_1 0:ea35c18c85fc 242 }
lilac0112_1 0:ea35c18c85fc 243 return;
lilac0112_1 0:ea35c18c85fc 244 }
lilac0112_1 0:ea35c18c85fc 245 void modeAttack2(void){
lilac0112_1 0:ea35c18c85fc 246 double ir_x, ir_y;
lilac0112_1 5:5ff3a7d5d8c2 247 int vx,vy,vs;
lilac0112_1 5:5ff3a7d5d8c2 248 /*int LineForce[2];
lilac0112_1 0:ea35c18c85fc 249 uint8_t LineDir[4];
lilac0112_1 0:ea35c18c85fc 250 uint8_t LineOn[4];
lilac0112_1 0:ea35c18c85fc 251 uint8_t LineReturn[4];
lilac0112_1 0:ea35c18c85fc 252 uint8_t LineStop[2];
lilac0112_1 0:ea35c18c85fc 253 uint8_t IrRange[4];
lilac0112_1 5:5ff3a7d5d8c2 254 uint8_t static LineBind[4];*/
lilac0112_1 0:ea35c18c85fc 255 //buint8_t static spi_count;
lilac0112_1 0:ea35c18c85fc 256 if(sys.KickOffFlag==1){
lilac0112_1 5:5ff3a7d5d8c2 257
lilac0112_1 5:5ff3a7d5d8c2 258 sys.MotorFlag=0;
lilac0112_1 5:5ff3a7d5d8c2 259 sys.IrFlag=0;
lilac0112_1 5:5ff3a7d5d8c2 260 /*LineBind[0]=0;
lilac0112_1 0:ea35c18c85fc 261 LineBind[1]=0;
lilac0112_1 0:ea35c18c85fc 262 LineBind[2]=0;
lilac0112_1 0:ea35c18c85fc 263 LineBind[3]=0;
lilac0112_1 0:ea35c18c85fc 264 LineReverseFlag=0;
lilac0112_1 0:ea35c18c85fc 265
lilac0112_1 0:ea35c18c85fc 266 LineSign[A_SPOT]=0;
lilac0112_1 0:ea35c18c85fc 267 LineSign[B_SPOT]=0;
lilac0112_1 0:ea35c18c85fc 268 LineSign[C_SPOT]=0;
lilac0112_1 0:ea35c18c85fc 269
lilac0112_1 2:635947de1583 270 data.lnFlag[A_SPOT]=0;
lilac0112_1 2:635947de1583 271 data.lnFlag[B_SPOT]=0;
lilac0112_1 2:635947de1583 272 data.lnFlag[C_SPOT]=0;
lilac0112_1 0:ea35c18c85fc 273
lilac0112_1 0:ea35c18c85fc 274 HmcResetFlag = 0;
lilac0112_1 0:ea35c18c85fc 275 PingFlag = 0;
lilac0112_1 0:ea35c18c85fc 276 //spi_count=0;
lilac0112_1 5:5ff3a7d5d8c2 277 */
lilac0112_1 0:ea35c18c85fc 278 hmc_reset=0;
lilac0112_1 0:ea35c18c85fc 279 sys.KickFlag = 0;
lilac0112_1 0:ea35c18c85fc 280
lilac0112_1 0:ea35c18c85fc 281 sys.KickOffFlag=0;
lilac0112_1 0:ea35c18c85fc 282 //while((Sw[2].read()==1)&&(Sw[3].read()==1));//押して離すとスタート
lilac0112_1 0:ea35c18c85fc 283 }
lilac0112_1 0:ea35c18c85fc 284 if(sys.IrFlag==1){
lilac0112_1 5:5ff3a7d5d8c2 285 //LED[0] = 0;
lilac0112_1 5:5ff3a7d5d8c2 286 //LED[1] = 1;
lilac0112_1 0:ea35c18c85fc 287 /*spi_count++;
lilac0112_1 0:ea35c18c85fc 288 if(spi_count%10 == 0){
lilac0112_1 0:ea35c18c85fc 289 ReadPing();
lilac0112_1 0:ea35c18c85fc 290 }
lilac0112_1 0:ea35c18c85fc 291 else{
lilac0112_1 0:ea35c18c85fc 292 ReadIr();
lilac0112_1 0:ea35c18c85fc 293 }
lilac0112_1 0:ea35c18c85fc 294 if(spi_count==20) spi_count=0;
lilac0112_1 0:ea35c18c85fc 295 */
lilac0112_1 0:ea35c18c85fc 296 ReadIr();
lilac0112_1 0:ea35c18c85fc 297 sys.IrFlag=0;
lilac0112_1 0:ea35c18c85fc 298 }
lilac0112_1 0:ea35c18c85fc 299 if(sys.PidFlag==1){
lilac0112_1 0:ea35c18c85fc 300 PidUpdate();
lilac0112_1 0:ea35c18c85fc 301 sys.PidFlag=0;
lilac0112_1 0:ea35c18c85fc 302 }
lilac0112_1 5:5ff3a7d5d8c2 303 /*if(PingFlag==1){
lilac0112_1 0:ea35c18c85fc 304 ReadPing();
lilac0112_1 0:ea35c18c85fc 305 PingFlag=0;
lilac0112_1 5:5ff3a7d5d8c2 306 }*/
lilac0112_1 0:ea35c18c85fc 307 /*
lilac0112_1 0:ea35c18c85fc 308 if(HmcResetFlag==1){
lilac0112_1 0:ea35c18c85fc 309 HmcReset();
lilac0112_1 0:ea35c18c85fc 310 HmcResetFlag=0;
lilac0112_1 0:ea35c18c85fc 311 }
lilac0112_1 0:ea35c18c85fc 312 */
lilac0112_1 5:5ff3a7d5d8c2 313 //ir_x = ir_move_val_old[data.irNotice][data.irPosition][IR_X];
lilac0112_1 5:5ff3a7d5d8c2 314 //ir_y = ir_move_val_old[data.irNotice][data.irPosition][IR_Y];
lilac0112_1 5:5ff3a7d5d8c2 315 ir_x = ir_move_val[0][data.irNotice][data.irPosition][IR_X_DIR]+ir_move_val[0][data.irNotice][data.irPosition][IR_X_TURN];
lilac0112_1 5:5ff3a7d5d8c2 316 ir_y = ir_move_val[0][data.irNotice][data.irPosition][IR_Y_DIR]+ir_move_val[0][data.irNotice][data.irPosition][IR_Y_TURN];
lilac0112_1 5:5ff3a7d5d8c2 317
lilac0112_1 2:635947de1583 318 if(data.irPosition<8){
lilac0112_1 0:ea35c18c85fc 319 ir_x *= sys.l_pow;
lilac0112_1 0:ea35c18c85fc 320 ir_y *= sys.l_pow;
lilac0112_1 0:ea35c18c85fc 321 }
lilac0112_1 0:ea35c18c85fc 322 else{
lilac0112_1 0:ea35c18c85fc 323 ir_x *= sys.s_pow;
lilac0112_1 0:ea35c18c85fc 324 ir_y *= sys.s_pow;
lilac0112_1 0:ea35c18c85fc 325 }
lilac0112_1 0:ea35c18c85fc 326
lilac0112_1 0:ea35c18c85fc 327 //Lineを考慮していないIrのみの値
lilac0112_1 0:ea35c18c85fc 328 vx = ir_x;
lilac0112_1 0:ea35c18c85fc 329 vy = ir_y;
lilac0112_1 5:5ff3a7d5d8c2 330 /*
lilac0112_1 2:635947de1583 331 if((data.irPosition==10)&&(vy>0)){
lilac0112_1 0:ea35c18c85fc 332 vy += 0;//前進加速
lilac0112_1 0:ea35c18c85fc 333 }
lilac0112_1 2:635947de1583 334 if((data.irPosition==11)&&(vy>0)){
lilac0112_1 0:ea35c18c85fc 335 vy += 0;//前進加速
lilac0112_1 0:ea35c18c85fc 336 if(sys.KickFlag==1){
lilac0112_1 0:ea35c18c85fc 337 DriveSolenoid();
lilac0112_1 0:ea35c18c85fc 338 }
lilac0112_1 0:ea35c18c85fc 339 }
lilac0112_1 2:635947de1583 340 if((data.irPosition==12)&&(vy>0)){
lilac0112_1 0:ea35c18c85fc 341 vy += 0;//前進加速
lilac0112_1 0:ea35c18c85fc 342 }
lilac0112_1 2:635947de1583 343 if((data.irPosition==1)&&(vy>0)){
lilac0112_1 0:ea35c18c85fc 344 vy += 0;//前進加速
lilac0112_1 0:ea35c18c85fc 345 }
lilac0112_1 2:635947de1583 346 if((data.irPosition==2)&&(vy>0)){
lilac0112_1 0:ea35c18c85fc 347 vy += 0;//前進加速
lilac0112_1 0:ea35c18c85fc 348 }
lilac0112_1 0:ea35c18c85fc 349
lilac0112_1 2:635947de1583 350 if((data.irPosition==17)&&(data.ping[L_PING]>data.ping[R_PING])){
lilac0112_1 0:ea35c18c85fc 351 vx *= -1.0;//背後回り込みの左右判断
lilac0112_1 5:5ff3a7d5d8c2 352 }*/
lilac0112_1 0:ea35c18c85fc 353 /*
lilac0112_1 0:ea35c18c85fc 354 if((cmps_set.InputPID<(REFERENCE-30))||(cmps_set.InputPID>(REFERENCE+30))){
lilac0112_1 0:ea35c18c85fc 355 vx = vx*(0.75);
lilac0112_1 0:ea35c18c85fc 356 vy = vy*(0.75);
lilac0112_1 0:ea35c18c85fc 357 }
lilac0112_1 0:ea35c18c85fc 358 */
lilac0112_1 0:ea35c18c85fc 359 //Lineを踏み始めた方向を調べる
lilac0112_1 2:635947de1583 360 /*LineDir[A_SPOT] = (vx>0)&&((data.lnFlag[A_SPOT]==1)&&(1 ))&&(LineFirst[X_AXIS] == A_SPOT);
lilac0112_1 2:635947de1583 361 LineDir[B_SPOT] = (vx<0)&&((data.lnFlag[B_SPOT]==1)&&(1 ))&&(LineFirst[X_AXIS] == B_SPOT);
lilac0112_1 2:635947de1583 362 LineDir[C_SPOT] = (vy<0)&&((data.lnFlag[C_SPOT]==1)&&(1 ))&&(LineFirst[Y_AXIS] == C_SPOT);
lilac0112_1 2:635947de1583 363 LineDir[AB_SPOT]= (vy>0)&&((data.lnFlag[A_SPOT]==1)&&(data.lnFlag[B_SPOT]==1))&&(LineFirst[Y_AXIS] == AB_SPOT);*/
lilac0112_1 5:5ff3a7d5d8c2 364 /*
lilac0112_1 0:ea35c18c85fc 365 LineDir[A_SPOT] = (vx>0)&&((LineSign[A_SPOT]==1)&&(1 ))&&(LineFirst[X_AXIS] == A_SPOT);
lilac0112_1 0:ea35c18c85fc 366 LineDir[B_SPOT] = (vx<0)&&((LineSign[B_SPOT]==1)&&(1 ))&&(LineFirst[X_AXIS] == B_SPOT);
lilac0112_1 0:ea35c18c85fc 367 LineDir[C_SPOT] = (vy<0)&&((LineSign[C_SPOT]==1)&&(1 ))&&(LineFirst[Y_AXIS] == C_SPOT);
lilac0112_1 0:ea35c18c85fc 368 LineDir[AB_SPOT]= (vy>0)&&((LineSign[A_SPOT]==1)&&(LineSign[B_SPOT]==1))&&(LineFirst[Y_AXIS] == AB_SPOT);
lilac0112_1 0:ea35c18c85fc 369
lilac0112_1 0:ea35c18c85fc 370 //Irボールの方向
lilac0112_1 0:ea35c18c85fc 371 //strict
lilac0112_1 0:ea35c18c85fc 372
lilac0112_1 2:635947de1583 373 IrRange[A_SPOT] = ((18<=data.irPosition)&&(data.irPosition<=19))||((8<=data.irPosition)&&(data.irPosition<=10))
lilac0112_1 2:635947de1583 374 ||((6<=data.irPosition)&&(data.irPosition<=7))||(( 1)&&(data.irPosition<=1));
lilac0112_1 2:635947de1583 375 IrRange[B_SPOT] = ((12<=data.irPosition)&&(data.irPosition<=16))||((2<=data.irPosition)&&(data.irPosition<=5));
lilac0112_1 2:635947de1583 376 IrRange[C_SPOT] = ((15<=data.irPosition)&&(data.irPosition<=19))||((4<=data.irPosition)&&(data.irPosition<=7));
lilac0112_1 2:635947de1583 377 IrRange[AB_SPOT] = ((9<=data.irPosition)&&(data.irPosition<=13))||(( 1)&&(data.irPosition<=3));
lilac0112_1 5:5ff3a7d5d8c2 378 */
lilac0112_1 0:ea35c18c85fc 379 //sweet
lilac0112_1 2:635947de1583 380 /*IrRange[A_SPOT] = ((19<=data.irPosition)&&(data.irPosition<=19))||((8<=data.irPosition)&&(data.irPosition<=9))
lilac0112_1 2:635947de1583 381 ||((6<=data.irPosition)&&(data.irPosition<=7))||(( 1)&&(data.irPosition<=1));
lilac0112_1 2:635947de1583 382 IrRange[B_SPOT] = ((13<=data.irPosition)&&(data.irPosition<=15))||((2<=data.irPosition)&&(data.irPosition<=5));
lilac0112_1 2:635947de1583 383 IrRange[C_SPOT] = ((16<=data.irPosition)&&(data.irPosition<=18))||((4<=data.irPosition)&&(data.irPosition<=7));
lilac0112_1 2:635947de1583 384 IrRange[AB_SPOT] = ((10<=data.irPosition)&&(data.irPosition<=12))||(( 1)&&(data.irPosition<=3));*/
lilac0112_1 5:5ff3a7d5d8c2 385 /*
lilac0112_1 0:ea35c18c85fc 386 //none
lilac0112_1 2:635947de1583 387 if(data.irNotice==IR_NONE){
lilac0112_1 0:ea35c18c85fc 388 IrRange[A_SPOT] = 0;
lilac0112_1 0:ea35c18c85fc 389 IrRange[B_SPOT] = 0;
lilac0112_1 0:ea35c18c85fc 390 IrRange[C_SPOT] = 0;
lilac0112_1 0:ea35c18c85fc 391 IrRange[AB_SPOT] = 0;
lilac0112_1 0:ea35c18c85fc 392 }
lilac0112_1 0:ea35c18c85fc 393 //白線を踏み始めた方向とボールの方向が一致．(SelfHold)
lilac0112_1 0:ea35c18c85fc 394 LineBind[A_SPOT] = (IrRange[A_SPOT]==1)&&((LineDir[A_SPOT]==1)||(LineBind[A_SPOT]==1));
lilac0112_1 0:ea35c18c85fc 395 LineBind[B_SPOT] = (IrRange[B_SPOT]==1)&&((LineDir[B_SPOT]==1)||(LineBind[B_SPOT]==1));
lilac0112_1 0:ea35c18c85fc 396 LineBind[C_SPOT] = (IrRange[C_SPOT]==1)&&((LineDir[C_SPOT]==1)||(LineBind[C_SPOT]==1));
lilac0112_1 0:ea35c18c85fc 397 LineBind[AB_SPOT] = (IrRange[AB_SPOT]==1)&&((LineDir[AB_SPOT]==1)||(LineBind[AB_SPOT]==1));
lilac0112_1 0:ea35c18c85fc 398
lilac0112_1 0:ea35c18c85fc 399 LineStop[X_AXIS] = (LineBind[A_SPOT]==0)*(LineBind[B_SPOT]==0);
lilac0112_1 0:ea35c18c85fc 400 LineStop[Y_AXIS] = (LineBind[C_SPOT]==0)*(LineBind[AB_SPOT]==0);
lilac0112_1 0:ea35c18c85fc 401
lilac0112_1 0:ea35c18c85fc 402 //白線踏んでる
lilac0112_1 2:635947de1583 403 if(LineRaw>0){
lilac0112_1 0:ea35c18c85fc 404 LineOn[A_SPOT] = (LineSign[A_SPOT]==1) &&(LineFirst[X_AXIS]==A_SPOT);
lilac0112_1 0:ea35c18c85fc 405 LineOn[B_SPOT] = (LineSign[B_SPOT]==1) &&(LineFirst[X_AXIS]==B_SPOT);
lilac0112_1 0:ea35c18c85fc 406 LineOn[C_SPOT] = (LineSign[C_SPOT]==1) &&(LineFirst[Y_AXIS]==C_SPOT);
lilac0112_1 0:ea35c18c85fc 407 LineOn[AB_SPOT] = ((LineSign[A_SPOT]==1)&&(LineSign[B_SPOT]==1))&&(LineFirst[Y_AXIS]==AB_SPOT);
lilac0112_1 0:ea35c18c85fc 408 //外側に向かう力を消す．
lilac0112_1 0:ea35c18c85fc 409 //x
lilac0112_1 0:ea35c18c85fc 410 if(((LineOn[A_SPOT]==1)&&(vx>0))||((LineOn[B_SPOT]==1)&&(vx<0))){
lilac0112_1 0:ea35c18c85fc 411 vx=0;
lilac0112_1 0:ea35c18c85fc 412 //yの力を加える．
lilac0112_1 0:ea35c18c85fc 413 if(vy>0){vy += 10;}
lilac0112_1 0:ea35c18c85fc 414 if(vy<0){vy -= 10;}
lilac0112_1 0:ea35c18c85fc 415 }
lilac0112_1 0:ea35c18c85fc 416 //y
lilac0112_1 0:ea35c18c85fc 417 if(((LineOn[C_SPOT]==1)&&(vy<0))||((LineOn[AB_SPOT]==1)&&(vy>0))){
lilac0112_1 0:ea35c18c85fc 418 vy=0;
lilac0112_1 0:ea35c18c85fc 419 }
lilac0112_1 0:ea35c18c85fc 420 //内側に向かう力を加える．
lilac0112_1 0:ea35c18c85fc 421 LineReturn[A_SPOT] = (LineOn[A_SPOT]==1);
lilac0112_1 0:ea35c18c85fc 422 if((LineReturn[A_SPOT]==1)&&(LineOn[AB_SPOT]==1)){
lilac0112_1 0:ea35c18c85fc 423 if(LineOn[C_SPOT]==0){
lilac0112_1 0:ea35c18c85fc 424 LineReturn[A_SPOT]=0;
lilac0112_1 0:ea35c18c85fc 425 }
lilac0112_1 0:ea35c18c85fc 426 else{
lilac0112_1 0:ea35c18c85fc 427 LineReturn[A_SPOT]=1;
lilac0112_1 0:ea35c18c85fc 428 }
lilac0112_1 0:ea35c18c85fc 429 }
lilac0112_1 0:ea35c18c85fc 430
lilac0112_1 0:ea35c18c85fc 431 LineReturn[B_SPOT] = (LineOn[B_SPOT]==1);
lilac0112_1 0:ea35c18c85fc 432 if((LineReturn[B_SPOT]==1)&&(LineOn[AB_SPOT]==1)){
lilac0112_1 0:ea35c18c85fc 433 if(LineOn[C_SPOT]==0){
lilac0112_1 0:ea35c18c85fc 434 LineReturn[B_SPOT]=0;
lilac0112_1 0:ea35c18c85fc 435 }
lilac0112_1 0:ea35c18c85fc 436 else{
lilac0112_1 0:ea35c18c85fc 437 LineReturn[B_SPOT]=1;
lilac0112_1 0:ea35c18c85fc 438 }
lilac0112_1 0:ea35c18c85fc 439 }
lilac0112_1 0:ea35c18c85fc 440 LineReturn[C_SPOT] = (LineOn[C_SPOT]==1);
lilac0112_1 0:ea35c18c85fc 441 if(LineReturn[C_SPOT]==1){
lilac0112_1 0:ea35c18c85fc 442 LineReturn[A_SPOT]=0;
lilac0112_1 0:ea35c18c85fc 443 LineReturn[B_SPOT]=0;
lilac0112_1 0:ea35c18c85fc 444 }
lilac0112_1 0:ea35c18c85fc 445 LineReturn[AB_SPOT] = (LineOn[AB_SPOT]==1);
lilac0112_1 0:ea35c18c85fc 446
lilac0112_1 0:ea35c18c85fc 447 LineForce[X_AXIS] = (LINE_RF*2)*((-1)*(LineReturn[A_SPOT]==1) + ( 1)*(LineReturn[B_SPOT]==1)) +
lilac0112_1 0:ea35c18c85fc 448 (LINE_RF*2)*(LineReturn[AB_SPOT]==1)*(LineReturn[A_SPOT]==0)*(LineReturn[B_SPOT]==0)
lilac0112_1 2:635947de1583 449 *(( 1)*(data.ping[L_PING]<40) + (-1)*(data.ping[R_PING]<40)) +
lilac0112_1 0:ea35c18c85fc 450 (LINE_RF*2)*(LineReturn[C_SPOT]==1)*(LineReturn[A_SPOT]==0)*(LineReturn[B_SPOT]==0)
lilac0112_1 2:635947de1583 451 *(( 1)*(data.ping[L_PING]<40) + (-1)*(data.ping[R_PING]<40));
lilac0112_1 0:ea35c18c85fc 452 LineForce[Y_AXIS] = (LINE_RF*2)*(( 1)*(LineReturn[C_SPOT]==1) + (-1)*(LineReturn[AB_SPOT]==1));
lilac0112_1 0:ea35c18c85fc 453
lilac0112_1 0:ea35c18c85fc 454 Line_timeout[A_SPOT].attach(&LineClear_A, LINE_DELAY);
lilac0112_1 0:ea35c18c85fc 455 Line_timeout[B_SPOT].attach(&LineClear_B, LINE_DELAY);
lilac0112_1 0:ea35c18c85fc 456 Line_timeout[C_SPOT].attach(&LineClear_C, LINE_DELAY);
lilac0112_1 0:ea35c18c85fc 457 }
lilac0112_1 0:ea35c18c85fc 458 else{
lilac0112_1 0:ea35c18c85fc 459 LineForce[X_AXIS] = 0;
lilac0112_1 0:ea35c18c85fc 460 LineForce[Y_AXIS] = 0;
lilac0112_1 0:ea35c18c85fc 461 }
lilac0112_1 5:5ff3a7d5d8c2 462 */
lilac0112_1 5:5ff3a7d5d8c2 463 //vx = vx*LineStop[X_AXIS] + LineForce[X_AXIS];
lilac0112_1 5:5ff3a7d5d8c2 464 //vy = vy*LineStop[Y_AXIS] + LineForce[Y_AXIS];
lilac0112_1 0:ea35c18c85fc 465 vs = cmps_set.OutputPID;
lilac0112_1 0:ea35c18c85fc 466 move(
lilac0112_1 0:ea35c18c85fc 467 vx,
lilac0112_1 0:ea35c18c85fc 468 vy,
lilac0112_1 0:ea35c18c85fc 469 vs
lilac0112_1 0:ea35c18c85fc 470 );
lilac0112_1 0:ea35c18c85fc 471 /*move(
lilac0112_1 0:ea35c18c85fc 472 0,
lilac0112_1 0:ea35c18c85fc 473 0,
lilac0112_1 0:ea35c18c85fc 474 10
lilac0112_1 0:ea35c18c85fc 475 );*/
lilac0112_1 0:ea35c18c85fc 476 if(sys.MotorFlag==1){
lilac0112_1 0:ea35c18c85fc 477 tx_motor();
lilac0112_1 0:ea35c18c85fc 478 sys.MotorFlag=0;
lilac0112_1 0:ea35c18c85fc 479 }
lilac0112_1 0:ea35c18c85fc 480 return;
lilac0112_1 0:ea35c18c85fc 481 }
lilac0112_1 0:ea35c18c85fc 482 void modeAttack3(void){
lilac0112_1 0:ea35c18c85fc 483 double ir_x, ir_y;
lilac0112_1 0:ea35c18c85fc 484 int vx,vy,vs, LineForce[2];
lilac0112_1 0:ea35c18c85fc 485 uint8_t LineDir[4];
lilac0112_1 0:ea35c18c85fc 486 uint8_t LineOn[4];
lilac0112_1 0:ea35c18c85fc 487 uint8_t LineReturn[4];
lilac0112_1 0:ea35c18c85fc 488 uint8_t LineStop[2];
lilac0112_1 0:ea35c18c85fc 489 uint8_t IrRange[4];
lilac0112_1 0:ea35c18c85fc 490 uint8_t static LineBind[4];
lilac0112_1 0:ea35c18c85fc 491 //buint8_t static spi_count;
lilac0112_1 0:ea35c18c85fc 492 if(sys.KickOffFlag==1){
lilac0112_1 0:ea35c18c85fc 493 LineBind[0]=0;
lilac0112_1 0:ea35c18c85fc 494 LineBind[1]=0;
lilac0112_1 0:ea35c18c85fc 495 LineBind[2]=0;
lilac0112_1 0:ea35c18c85fc 496 LineBind[3]=0;
lilac0112_1 0:ea35c18c85fc 497 LineReverseFlag=0;
lilac0112_1 0:ea35c18c85fc 498
lilac0112_1 0:ea35c18c85fc 499 LineSign[A_SPOT]=0;
lilac0112_1 0:ea35c18c85fc 500 LineSign[B_SPOT]=0;
lilac0112_1 0:ea35c18c85fc 501 LineSign[C_SPOT]=0;
lilac0112_1 0:ea35c18c85fc 502
lilac0112_1 2:635947de1583 503 data.lnFlag[A_SPOT]=0;
lilac0112_1 2:635947de1583 504 data.lnFlag[B_SPOT]=0;
lilac0112_1 2:635947de1583 505 data.lnFlag[C_SPOT]=0;
lilac0112_1 0:ea35c18c85fc 506
lilac0112_1 0:ea35c18c85fc 507 LinePriority[X_AXIS]=0;
lilac0112_1 0:ea35c18c85fc 508 LinePriority[Y_AXIS]=0;
lilac0112_1 0:ea35c18c85fc 509
lilac0112_1 0:ea35c18c85fc 510 HmcResetFlag = 0;
lilac0112_1 0:ea35c18c85fc 511 PingFlag = 0;
lilac0112_1 0:ea35c18c85fc 512 //spi_count=0;
lilac0112_1 0:ea35c18c85fc 513
lilac0112_1 0:ea35c18c85fc 514 hmc_reset=0;
lilac0112_1 0:ea35c18c85fc 515 sys.KickFlag = 0;
lilac0112_1 0:ea35c18c85fc 516
lilac0112_1 0:ea35c18c85fc 517 sys.KickOffFlag=0;
lilac0112_1 0:ea35c18c85fc 518 //while((Sw[2].read()==1)&&(Sw[3].read()==1));//押して離すとスタート
lilac0112_1 0:ea35c18c85fc 519 }
lilac0112_1 0:ea35c18c85fc 520 if(sys.IrFlag==1){
lilac0112_1 0:ea35c18c85fc 521 /*spi_count++;
lilac0112_1 0:ea35c18c85fc 522 if(spi_count%10 == 0){
lilac0112_1 0:ea35c18c85fc 523 ReadPing();
lilac0112_1 0:ea35c18c85fc 524 }
lilac0112_1 0:ea35c18c85fc 525 else{
lilac0112_1 0:ea35c18c85fc 526 ReadIr();
lilac0112_1 0:ea35c18c85fc 527 }
lilac0112_1 0:ea35c18c85fc 528 if(spi_count==20) spi_count=0;
lilac0112_1 0:ea35c18c85fc 529 */
lilac0112_1 0:ea35c18c85fc 530 ReadIr();
lilac0112_1 0:ea35c18c85fc 531 sys.IrFlag=0;
lilac0112_1 0:ea35c18c85fc 532 }
lilac0112_1 0:ea35c18c85fc 533 if(sys.PidFlag==1){
lilac0112_1 0:ea35c18c85fc 534 PidUpdate();
lilac0112_1 0:ea35c18c85fc 535 sys.PidFlag=0;
lilac0112_1 0:ea35c18c85fc 536 }
lilac0112_1 0:ea35c18c85fc 537 if(PingFlag==1){
lilac0112_1 0:ea35c18c85fc 538 ReadPing();
lilac0112_1 0:ea35c18c85fc 539 PingFlag=0;
lilac0112_1 0:ea35c18c85fc 540 }
lilac0112_1 0:ea35c18c85fc 541 /*
lilac0112_1 0:ea35c18c85fc 542 if(HmcResetFlag==1){
lilac0112_1 0:ea35c18c85fc 543 HmcReset();
lilac0112_1 0:ea35c18c85fc 544 HmcResetFlag=0;
lilac0112_1 0:ea35c18c85fc 545 }
lilac0112_1 0:ea35c18c85fc 546 */
lilac0112_1 5:5ff3a7d5d8c2 547 ir_x = ir_move_val_old[data.irNotice][data.irPosition][IR_X];
lilac0112_1 5:5ff3a7d5d8c2 548 ir_y = ir_move_val_old[data.irNotice][data.irPosition][IR_Y];
lilac0112_1 2:635947de1583 549 if(data.irPosition<8){
lilac0112_1 0:ea35c18c85fc 550 ir_x *= sys.l_pow;
lilac0112_1 0:ea35c18c85fc 551 ir_y *= sys.l_pow;
lilac0112_1 0:ea35c18c85fc 552 }
lilac0112_1 0:ea35c18c85fc 553 else{
lilac0112_1 0:ea35c18c85fc 554 ir_x *= sys.s_pow;
lilac0112_1 0:ea35c18c85fc 555 ir_y *= sys.s_pow;
lilac0112_1 0:ea35c18c85fc 556 }
lilac0112_1 0:ea35c18c85fc 557
lilac0112_1 0:ea35c18c85fc 558 //Lineを考慮していないIrのみの値
lilac0112_1 0:ea35c18c85fc 559 vx = ir_x;
lilac0112_1 0:ea35c18c85fc 560 vy = ir_y;
lilac0112_1 0:ea35c18c85fc 561
lilac0112_1 2:635947de1583 562 if((data.irPosition==10)&&(vy>0)){
lilac0112_1 0:ea35c18c85fc 563 vy += 15;//前進加速
lilac0112_1 0:ea35c18c85fc 564 }
lilac0112_1 2:635947de1583 565 if((data.irPosition==11)&&(vy>0)){
lilac0112_1 0:ea35c18c85fc 566 vy += 15;//前進加速
lilac0112_1 0:ea35c18c85fc 567 if(sys.KickFlag==1){
lilac0112_1 0:ea35c18c85fc 568 DriveSolenoid();
lilac0112_1 0:ea35c18c85fc 569 }
lilac0112_1 0:ea35c18c85fc 570 }
lilac0112_1 2:635947de1583 571 if((data.irPosition==12)&&(vy>0)){
lilac0112_1 0:ea35c18c85fc 572 vy += 15;//前進加速
lilac0112_1 0:ea35c18c85fc 573 }
lilac0112_1 2:635947de1583 574 if((data.irPosition==1)&&(vy>0)){
lilac0112_1 0:ea35c18c85fc 575 vy += 25;//前進加速
lilac0112_1 0:ea35c18c85fc 576 }
lilac0112_1 2:635947de1583 577 if((data.irPosition==2)&&(vy>0)){
lilac0112_1 0:ea35c18c85fc 578 vy += 25;//前進加速
lilac0112_1 0:ea35c18c85fc 579 }
lilac0112_1 0:ea35c18c85fc 580
lilac0112_1 0:ea35c18c85fc 581
lilac0112_1 2:635947de1583 582 if((data.irPosition==17)&&(data.ping[L_PING]>data.ping[R_PING])){
lilac0112_1 0:ea35c18c85fc 583 vx *= -1.0;//背後回り込みの左右判断
lilac0112_1 0:ea35c18c85fc 584 }
lilac0112_1 0:ea35c18c85fc 585 /*
lilac0112_1 0:ea35c18c85fc 586 if((cmps_set.InputPID<(REFERENCE-30))||(cmps_set.InputPID>(REFERENCE+30))){
lilac0112_1 0:ea35c18c85fc 587 vx = vx*(0.75);
lilac0112_1 0:ea35c18c85fc 588 vy = vy*(0.75);
lilac0112_1 0:ea35c18c85fc 589 }
lilac0112_1 0:ea35c18c85fc 590 */
lilac0112_1 0:ea35c18c85fc 591 //Lineを踏み始めた方向を調べる
lilac0112_1 2:635947de1583 592 /*LineDir[A_SPOT] = (vx>0)&&((data.lnFlag[A_SPOT]==1)&&(1 ))&&(LineFirst[X_AXIS] == A_SPOT);
lilac0112_1 2:635947de1583 593 LineDir[B_SPOT] = (vx<0)&&((data.lnFlag[B_SPOT]==1)&&(1 ))&&(LineFirst[X_AXIS] == B_SPOT);
lilac0112_1 2:635947de1583 594 LineDir[C_SPOT] = (vy<0)&&((data.lnFlag[C_SPOT]==1)&&(1 ))&&(LineFirst[Y_AXIS] == C_SPOT);
lilac0112_1 2:635947de1583 595 LineDir[AB_SPOT]= (vy>0)&&((data.lnFlag[A_SPOT]==1)&&(data.lnFlag[B_SPOT]==1))&&(LineFirst[Y_AXIS] == AB_SPOT);*/
lilac0112_1 0:ea35c18c85fc 596
lilac0112_1 0:ea35c18c85fc 597 LineDir[A_SPOT] = (vx>0)&&((LineSign[A_SPOT]==1)&&(1 ))&&(LineFirst[X_AXIS] == A_SPOT);
lilac0112_1 0:ea35c18c85fc 598 LineDir[B_SPOT] = (vx<0)&&((LineSign[B_SPOT]==1)&&(1 ))&&(LineFirst[X_AXIS] == B_SPOT);
lilac0112_1 0:ea35c18c85fc 599 LineDir[C_SPOT] = (vy<0)&&((LineSign[C_SPOT]==1)&&(1 ))&&(LineFirst[Y_AXIS] == C_SPOT);
lilac0112_1 0:ea35c18c85fc 600 LineDir[AB_SPOT]= (vy>0)&&((LineSign[A_SPOT]==1)&&(LineSign[B_SPOT]==1))&&(LineFirst[Y_AXIS] == AB_SPOT);
lilac0112_1 0:ea35c18c85fc 601
lilac0112_1 0:ea35c18c85fc 602 //Irボールの方向
lilac0112_1 0:ea35c18c85fc 603 //strict
lilac0112_1 0:ea35c18c85fc 604
lilac0112_1 2:635947de1583 605 IrRange[A_SPOT] = ((18<=data.irPosition)&&(data.irPosition<=19))||((8<=data.irPosition)&&(data.irPosition<=10))
lilac0112_1 2:635947de1583 606 ||((6<=data.irPosition)&&(data.irPosition<=7))||(( 1)&&(data.irPosition<=1));
lilac0112_1 2:635947de1583 607 IrRange[B_SPOT] = ((12<=data.irPosition)&&(data.irPosition<=16))||((2<=data.irPosition)&&(data.irPosition<=5));
lilac0112_1 2:635947de1583 608 IrRange[C_SPOT] = ((15<=data.irPosition)&&(data.irPosition<=19))||((4<=data.irPosition)&&(data.irPosition<=7));
lilac0112_1 2:635947de1583 609 IrRange[AB_SPOT] = ((9<=data.irPosition)&&(data.irPosition<=13))||(( 1)&&(data.irPosition<=3));
lilac0112_1 0:ea35c18c85fc 610
lilac0112_1 0:ea35c18c85fc 611 //sweet
lilac0112_1 2:635947de1583 612 /*IrRange[A_SPOT] = ((19<=data.irPosition)&&(data.irPosition<=19))||((8<=data.irPosition)&&(data.irPosition<=9))
lilac0112_1 2:635947de1583 613 ||((6<=data.irPosition)&&(data.irPosition<=7))||(( 1)&&(data.irPosition<=1));
lilac0112_1 2:635947de1583 614 IrRange[B_SPOT] = ((13<=data.irPosition)&&(data.irPosition<=15))||((2<=data.irPosition)&&(data.irPosition<=5));
lilac0112_1 2:635947de1583 615 IrRange[C_SPOT] = ((16<=data.irPosition)&&(data.irPosition<=18))||((4<=data.irPosition)&&(data.irPosition<=7));
lilac0112_1 2:635947de1583 616 IrRange[AB_SPOT] = ((10<=data.irPosition)&&(data.irPosition<=12))||(( 1)&&(data.irPosition<=3));*/
lilac0112_1 0:ea35c18c85fc 617 //none
lilac0112_1 2:635947de1583 618 if(data.irNotice==IR_NONE){
lilac0112_1 0:ea35c18c85fc 619 IrRange[A_SPOT] = 0;
lilac0112_1 0:ea35c18c85fc 620 IrRange[B_SPOT] = 0;
lilac0112_1 0:ea35c18c85fc 621 IrRange[C_SPOT] = 0;
lilac0112_1 0:ea35c18c85fc 622 IrRange[AB_SPOT] = 0;
lilac0112_1 0:ea35c18c85fc 623 }
lilac0112_1 0:ea35c18c85fc 624 //白線を踏み始めた方向とボールの方向が一致．(SelfHold)
lilac0112_1 0:ea35c18c85fc 625 LineBind[A_SPOT] = (IrRange[A_SPOT]==1)&&((LineDir[A_SPOT]==1)||(LineBind[A_SPOT]==1));
lilac0112_1 0:ea35c18c85fc 626 LineBind[B_SPOT] = (IrRange[B_SPOT]==1)&&((LineDir[B_SPOT]==1)||(LineBind[B_SPOT]==1));
lilac0112_1 0:ea35c18c85fc 627 LineBind[C_SPOT] = (IrRange[C_SPOT]==1)&&((LineDir[C_SPOT]==1)||(LineBind[C_SPOT]==1));
lilac0112_1 0:ea35c18c85fc 628 LineBind[AB_SPOT] = (IrRange[AB_SPOT]==1)&&((LineDir[AB_SPOT]==1)||(LineBind[AB_SPOT]==1));
lilac0112_1 0:ea35c18c85fc 629
lilac0112_1 0:ea35c18c85fc 630 LineStop[X_AXIS] = 1;//(LineBind[A_SPOT]==0)*(LineBind[B_SPOT]==0);
lilac0112_1 0:ea35c18c85fc 631 LineStop[Y_AXIS] = 1;//(LineBind[C_SPOT]==0)*(LineBind[AB_SPOT]==0);
lilac0112_1 0:ea35c18c85fc 632
lilac0112_1 0:ea35c18c85fc 633 //白線踏んでる
lilac0112_1 2:635947de1583 634 if(LineRaw>0){
lilac0112_1 0:ea35c18c85fc 635 LineOn[A_SPOT] = (LineSign[A_SPOT]==1) &&(LineFirst[X_AXIS]==A_SPOT);
lilac0112_1 0:ea35c18c85fc 636 LineOn[B_SPOT] = (LineSign[B_SPOT]==1) &&(LineFirst[X_AXIS]==B_SPOT);
lilac0112_1 0:ea35c18c85fc 637 LineOn[C_SPOT] = (LineSign[C_SPOT]==1) &&(LineFirst[Y_AXIS]==C_SPOT);
lilac0112_1 0:ea35c18c85fc 638 LineOn[AB_SPOT] = ((LineSign[A_SPOT]==1)&&(LineSign[B_SPOT]==1))&&(LineFirst[Y_AXIS]==AB_SPOT);
lilac0112_1 0:ea35c18c85fc 639 //外側に向かう力を消す．
lilac0112_1 0:ea35c18c85fc 640 //x
lilac0112_1 0:ea35c18c85fc 641 if(((LineOn[A_SPOT]==1)&&(vx>0))||((LineOn[B_SPOT]==1)&&(vx<0))){
lilac0112_1 0:ea35c18c85fc 642
lilac0112_1 0:ea35c18c85fc 643 if(LinePriority[Y_AXIS]==0){
lilac0112_1 0:ea35c18c85fc 644 LinePriority[X_AXIS]=1;
lilac0112_1 0:ea35c18c85fc 645 LinePriority[Y_AXIS]=0;
lilac0112_1 0:ea35c18c85fc 646 }
lilac0112_1 0:ea35c18c85fc 647 vx=0;
lilac0112_1 0:ea35c18c85fc 648 //yの力を加える．
lilac0112_1 0:ea35c18c85fc 649 if(vy>0){vy += 10;}
lilac0112_1 0:ea35c18c85fc 650 if(vy<0){vy -= 10;}
lilac0112_1 0:ea35c18c85fc 651 }
lilac0112_1 0:ea35c18c85fc 652 //y
lilac0112_1 0:ea35c18c85fc 653 if(((LineOn[C_SPOT]==1)&&(vy<0))||((LineOn[AB_SPOT]==1)&&(vy>0))){
lilac0112_1 0:ea35c18c85fc 654 if(LinePriority[X_AXIS]==0){
lilac0112_1 0:ea35c18c85fc 655 LinePriority[X_AXIS]=0;
lilac0112_1 0:ea35c18c85fc 656 LinePriority[Y_AXIS]=1;
lilac0112_1 0:ea35c18c85fc 657 }
lilac0112_1 0:ea35c18c85fc 658 if((LinePriority[X_AXIS]==1)&&(LineOn[AB_SPOT]==1)){
lilac0112_1 0:ea35c18c85fc 659 LinePriority[X_AXIS]=0;
lilac0112_1 0:ea35c18c85fc 660 LinePriority[Y_AXIS]=1;
lilac0112_1 0:ea35c18c85fc 661 }
lilac0112_1 0:ea35c18c85fc 662 vy=0;
lilac0112_1 0:ea35c18c85fc 663 }
lilac0112_1 0:ea35c18c85fc 664 //内側に向かう力を加える．
lilac0112_1 0:ea35c18c85fc 665 LineReturn[A_SPOT] = (LineOn[A_SPOT]==1);
lilac0112_1 0:ea35c18c85fc 666 /*if((LineReturn[A_SPOT]==1)&&(LineOn[AB_SPOT]==1)){
lilac0112_1 0:ea35c18c85fc 667 if(LineOn[C_SPOT]==0){
lilac0112_1 0:ea35c18c85fc 668 LineReturn[A_SPOT]=0;
lilac0112_1 0:ea35c18c85fc 669 }
lilac0112_1 0:ea35c18c85fc 670 else{
lilac0112_1 0:ea35c18c85fc 671 LineReturn[A_SPOT]=1;
lilac0112_1 0:ea35c18c85fc 672 }
lilac0112_1 0:ea35c18c85fc 673 }*/
lilac0112_1 0:ea35c18c85fc 674
lilac0112_1 0:ea35c18c85fc 675 LineReturn[B_SPOT] = (LineOn[B_SPOT]==1);
lilac0112_1 0:ea35c18c85fc 676 /*if((LineReturn[B_SPOT]==1)&&(LineOn[AB_SPOT]==1)){
lilac0112_1 0:ea35c18c85fc 677 if(LineOn[C_SPOT]==0){
lilac0112_1 0:ea35c18c85fc 678 LineReturn[B_SPOT]=0;
lilac0112_1 0:ea35c18c85fc 679 }
lilac0112_1 0:ea35c18c85fc 680 else{
lilac0112_1 0:ea35c18c85fc 681 LineReturn[B_SPOT]=1;
lilac0112_1 0:ea35c18c85fc 682 }
lilac0112_1 0:ea35c18c85fc 683 }*/
lilac0112_1 0:ea35c18c85fc 684 LineReturn[C_SPOT] = (LineOn[C_SPOT]==1);
lilac0112_1 0:ea35c18c85fc 685 /*if(LineReturn[C_SPOT]==1){
lilac0112_1 0:ea35c18c85fc 686 LineReturn[A_SPOT]=0;
lilac0112_1 0:ea35c18c85fc 687 LineReturn[B_SPOT]=0;
lilac0112_1 0:ea35c18c85fc 688 }*/
lilac0112_1 0:ea35c18c85fc 689 LineReturn[AB_SPOT] = (LineOn[AB_SPOT]==1);
lilac0112_1 0:ea35c18c85fc 690
lilac0112_1 0:ea35c18c85fc 691 LineForce[X_AXIS] = (LINE_RF*2)*(LinePriority[X_AXIS])*((-1)*(LineReturn[A_SPOT]==1) + ( 1)*(LineReturn[B_SPOT]==1)) +
lilac0112_1 2:635947de1583 692 (LINE_RF*2)*(LineReturn[AB_SPOT]==1)*(( 1)*(data.ping[L_PING]<40) + (-1)*(data.ping[R_PING]<40)) +
lilac0112_1 2:635947de1583 693 (LINE_RF*2)*(LineReturn[C_SPOT]==1)*(( 1)*(data.ping[L_PING]<40) + (-1)*(data.ping[R_PING]<40));
lilac0112_1 0:ea35c18c85fc 694 LineForce[Y_AXIS] = (LINE_RF*2)*(LinePriority[Y_AXIS])*(( 1)*(LineReturn[C_SPOT]==1) + (-1)*(LineReturn[AB_SPOT]==1));
lilac0112_1 0:ea35c18c85fc 695 /*
lilac0112_1 0:ea35c18c85fc 696 LineForce[X_AXIS] = (LINE_RF*2)*((-1)*(LineReturn[A_SPOT]==1) + ( 1)*(LineReturn[B_SPOT]==1)) +
lilac0112_1 0:ea35c18c85fc 697 (LINE_RF*2)*(LineReturn[AB_SPOT]==1)*(LineReturn[A_SPOT]==0)*(LineReturn[B_SPOT]==0)
lilac0112_1 2:635947de1583 698 *(( 1)*(data.ping[L_PING]<30) + (-1)*(data.ping[R_PING]<30)) +
lilac0112_1 0:ea35c18c85fc 699 (LINE_RF*2)*(LineReturn[C_SPOT]==1)*(LineReturn[A_SPOT]==0)*(LineReturn[B_SPOT]==0)
lilac0112_1 2:635947de1583 700 *(( 1)*(data.ping[L_PING]<30) + (-1)*(data.ping[R_PING]<30));
lilac0112_1 0:ea35c18c85fc 701 LineForce[Y_AXIS] = (LINE_RF*2)*(( 1)*(LineReturn[C_SPOT]==1) + (-1)*(LineReturn[AB_SPOT]==1));
lilac0112_1 0:ea35c18c85fc 702 */
lilac0112_1 0:ea35c18c85fc 703 Line_timeout[A_SPOT].attach(&LineClear_A, LINE_DELAY);
lilac0112_1 0:ea35c18c85fc 704 Line_timeout[B_SPOT].attach(&LineClear_B, LINE_DELAY);
lilac0112_1 0:ea35c18c85fc 705 Line_timeout[C_SPOT].attach(&LineClear_C, LINE_DELAY);
lilac0112_1 0:ea35c18c85fc 706 }
lilac0112_1 0:ea35c18c85fc 707 else{
lilac0112_1 0:ea35c18c85fc 708 LineForce[X_AXIS] = 0;
lilac0112_1 0:ea35c18c85fc 709 LineForce[Y_AXIS] = 0;
lilac0112_1 0:ea35c18c85fc 710
lilac0112_1 0:ea35c18c85fc 711 LinePriority[X_AXIS]=0;
lilac0112_1 0:ea35c18c85fc 712 LinePriority[Y_AXIS]=0;
lilac0112_1 0:ea35c18c85fc 713 }
lilac0112_1 0:ea35c18c85fc 714
lilac0112_1 0:ea35c18c85fc 715 vx = vx*LineStop[X_AXIS] + LineForce[X_AXIS];
lilac0112_1 0:ea35c18c85fc 716 vy = vy*LineStop[Y_AXIS] + LineForce[Y_AXIS];
lilac0112_1 0:ea35c18c85fc 717 vs = cmps_set.OutputPID;
lilac0112_1 0:ea35c18c85fc 718 move(
lilac0112_1 0:ea35c18c85fc 719 vx,
lilac0112_1 0:ea35c18c85fc 720 vy,
lilac0112_1 0:ea35c18c85fc 721 vs
lilac0112_1 0:ea35c18c85fc 722 );
lilac0112_1 0:ea35c18c85fc 723 /*move(
lilac0112_1 0:ea35c18c85fc 724 10,
lilac0112_1 0:ea35c18c85fc 725 0,
lilac0112_1 0:ea35c18c85fc 726 0
lilac0112_1 0:ea35c18c85fc 727 );*/
lilac0112_1 0:ea35c18c85fc 728 if(sys.MotorFlag==1){
lilac0112_1 0:ea35c18c85fc 729 tx_motor();
lilac0112_1 0:ea35c18c85fc 730 sys.MotorFlag=0;
lilac0112_1 0:ea35c18c85fc 731 }
lilac0112_1 0:ea35c18c85fc 732 if(sys.stopflag==1){
lilac0112_1 0:ea35c18c85fc 733 //停止処理
lilac0112_1 0:ea35c18c85fc 734 }
lilac0112_1 0:ea35c18c85fc 735 return;
lilac0112_1 0:ea35c18c85fc 736 }
lilac0112_1 0:ea35c18c85fc 737 void modeAttack4(void){
lilac0112_1 0:ea35c18c85fc 738 if(sys.KickFlag==1){
lilac0112_1 0:ea35c18c85fc 739 DriveSolenoid();
lilac0112_1 0:ea35c18c85fc 740 }
lilac0112_1 0:ea35c18c85fc 741 if(sys.stopflag==1){
lilac0112_1 0:ea35c18c85fc 742 //停止処理
lilac0112_1 0:ea35c18c85fc 743 }
lilac0112_1 0:ea35c18c85fc 744 return;
lilac0112_1 0:ea35c18c85fc 745 }
lilac0112_1 0:ea35c18c85fc 746 void modeAttack5(void){
lilac0112_1 0:ea35c18c85fc 747 if(sys.IrFlag==1){
lilac0112_1 0:ea35c18c85fc 748 ReadIr();
lilac0112_1 0:ea35c18c85fc 749 sys.IrFlag=0;
lilac0112_1 0:ea35c18c85fc 750 }
lilac0112_1 0:ea35c18c85fc 751 if(sys.PidFlag==1){
lilac0112_1 0:ea35c18c85fc 752 PidUpdate();
lilac0112_1 0:ea35c18c85fc 753 sys.PidFlag=0;
lilac0112_1 0:ea35c18c85fc 754 }
lilac0112_1 0:ea35c18c85fc 755 move(0,0,10);
lilac0112_1 0:ea35c18c85fc 756 if(sys.MotorFlag==1){
lilac0112_1 5:5ff3a7d5d8c2 757 LED[0] = 1;
lilac0112_1 5:5ff3a7d5d8c2 758 LED[1] = 0;
lilac0112_1 0:ea35c18c85fc 759 tx_motor();
lilac0112_1 0:ea35c18c85fc 760 sys.MotorFlag=0;
lilac0112_1 0:ea35c18c85fc 761 }
lilac0112_1 5:5ff3a7d5d8c2 762 else{
lilac0112_1 5:5ff3a7d5d8c2 763 LED[0] = 0;
lilac0112_1 5:5ff3a7d5d8c2 764 LED[1] = 1;
lilac0112_1 5:5ff3a7d5d8c2 765 }
lilac0112_1 0:ea35c18c85fc 766 if(sys.stopflag==1){
lilac0112_1 0:ea35c18c85fc 767 //停止処理
lilac0112_1 0:ea35c18c85fc 768 }
lilac0112_1 0:ea35c18c85fc 769 return;
lilac0112_1 0:ea35c18c85fc 770 }