CatPot 2015-2016 / Mbed 2 deprecated CatPot_2v10_T_Main

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

Dependencies:   mbed AQM1602 HMC6352 PID

main_processing/strategy/strategy.cpp@15:17502a27a60b, 2016-03-11 (annotated)

Committer:
lilac0112_1
Date:
Fri Mar 11 05:21:26 2016 +0000
Revision:
15:17502a27a60b
Parent:
14:b510adcb6065
Child:
16:4fadb7a87497
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 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 10:6df631c39f9b 58 hmc_reset=HMC_RST;
lilac0112_1 0:ea35c18c85fc 59 wait_us(100);
lilac0112_1 10:6df631c39f9b 60 hmc_reset=HMC_RUN;
lilac0112_1 0:ea35c18c85fc 61 }
lilac0112_1 0:ea35c18c85fc 62 uint8_t LineReverseFlag;
lilac0112_1 0:ea35c18c85fc 63 void LineReverse(void){
lilac0112_1 0:ea35c18c85fc 64 LineReverseFlag=0;
lilac0112_1 0:ea35c18c85fc 65 }
lilac0112_1 0:ea35c18c85fc 66 uint8_t LineSign[3];
lilac0112_1 0:ea35c18c85fc 67 uint8_t LineFirst[2];
lilac0112_1 0:ea35c18c85fc 68 uint8_t LinePriority[2];//0を後に，1を優先
lilac0112_1 2:635947de1583 69 void LineClear_A(void){LineSign[A_SPOT]=0;data.lnFlag[A_SPOT]=0;}
lilac0112_1 2:635947de1583 70 void LineClear_B(void){LineSign[B_SPOT]=0;data.lnFlag[B_SPOT]=0;}
lilac0112_1 2:635947de1583 71 void LineClear_C(void){LineSign[C_SPOT]=0;data.lnFlag[C_SPOT]=0;}
lilac0112_1 0:ea35c18c85fc 72 void LineCall_A(void){
lilac0112_1 0:ea35c18c85fc 73 //
lilac0112_1 0:ea35c18c85fc 74 LineSign[A_SPOT] = 1;
lilac0112_1 0:ea35c18c85fc 75 if(LineSign[B_SPOT]==0){
lilac0112_1 0:ea35c18c85fc 76 LineFirst[X_AXIS] = A_SPOT;
lilac0112_1 0:ea35c18c85fc 77 }
lilac0112_1 0:ea35c18c85fc 78 if((LineSign[A_SPOT]==1)&&(LineSign[B_SPOT]==1)){
lilac0112_1 0:ea35c18c85fc 79 if(LineSign[C_SPOT]==0){
lilac0112_1 0:ea35c18c85fc 80 LineFirst[Y_AXIS] = AB_SPOT;
lilac0112_1 0:ea35c18c85fc 81 }
lilac0112_1 0:ea35c18c85fc 82 }
lilac0112_1 0:ea35c18c85fc 83 //
lilac0112_1 2:635947de1583 84 if((Line[A_SPOT].read()==1)||(1)) data.lnFlag[A_SPOT]=1;
lilac0112_1 0:ea35c18c85fc 85 //
lilac0112_1 0:ea35c18c85fc 86 Line_timeout[A_SPOT].attach(&LineClear_A, LINE_DELAY);
lilac0112_1 0:ea35c18c85fc 87 }
lilac0112_1 0:ea35c18c85fc 88 void LineCall_B(void){
lilac0112_1 0:ea35c18c85fc 89 //
lilac0112_1 0:ea35c18c85fc 90 LineSign[B_SPOT] = 1;
lilac0112_1 0:ea35c18c85fc 91 if(LineSign[A_SPOT]==0){
lilac0112_1 0:ea35c18c85fc 92 LineFirst[X_AXIS] = B_SPOT;
lilac0112_1 0:ea35c18c85fc 93 }
lilac0112_1 0:ea35c18c85fc 94 if((LineSign[A_SPOT]==1)&&(LineSign[B_SPOT]==1)){
lilac0112_1 0:ea35c18c85fc 95 if(LineSign[C_SPOT]==0){
lilac0112_1 0:ea35c18c85fc 96 LineFirst[Y_AXIS] = AB_SPOT;
lilac0112_1 0:ea35c18c85fc 97 }
lilac0112_1 0:ea35c18c85fc 98 }
lilac0112_1 0:ea35c18c85fc 99 //
lilac0112_1 2:635947de1583 100 if((Line[B_SPOT].read()==1)||(1)) data.lnFlag[B_SPOT]=1;
lilac0112_1 0:ea35c18c85fc 101 //
lilac0112_1 0:ea35c18c85fc 102 Line_timeout[B_SPOT].attach(&LineClear_B, LINE_DELAY);
lilac0112_1 0:ea35c18c85fc 103 }
lilac0112_1 0:ea35c18c85fc 104 void LineCall_C(void){
lilac0112_1 0:ea35c18c85fc 105 //
lilac0112_1 0:ea35c18c85fc 106 LineSign[C_SPOT] = 1;
lilac0112_1 0:ea35c18c85fc 107 if(!((LineSign[A_SPOT]==1)&&(LineSign[B_SPOT]==1))){
lilac0112_1 0:ea35c18c85fc 108 LineFirst[Y_AXIS] = C_SPOT;
lilac0112_1 0:ea35c18c85fc 109 }
lilac0112_1 0:ea35c18c85fc 110 //
lilac0112_1 2:635947de1583 111 if((Line[C_SPOT].read()==1)||(1)) data.lnFlag[C_SPOT]=1;
lilac0112_1 0:ea35c18c85fc 112 //
lilac0112_1 0:ea35c18c85fc 113 Line_timeout[C_SPOT].attach(&LineClear_C, LINE_DELAY);
lilac0112_1 0:ea35c18c85fc 114 }
lilac0112_1 0:ea35c18c85fc 115 void modeAttack1(void){
lilac0112_1 0:ea35c18c85fc 116 double ir_x, ir_y;
lilac0112_1 0:ea35c18c85fc 117 int vx,vy,vs;
lilac0112_1 0:ea35c18c85fc 118 uint8_t LineDir[4];
lilac0112_1 0:ea35c18c85fc 119 uint8_t LineStop[2];
lilac0112_1 0:ea35c18c85fc 120 uint8_t IrRange[4];
lilac0112_1 0:ea35c18c85fc 121 uint8_t LinePulse[4];
lilac0112_1 0:ea35c18c85fc 122 uint8_t static LineBind[4];
lilac0112_1 0:ea35c18c85fc 123 if(sys.KickOffFlag==1){
lilac0112_1 0:ea35c18c85fc 124 LineBind[0]=0;
lilac0112_1 0:ea35c18c85fc 125 LineBind[1]=0;
lilac0112_1 0:ea35c18c85fc 126 LineBind[2]=0;
lilac0112_1 0:ea35c18c85fc 127 LineBind[3]=0;
lilac0112_1 0:ea35c18c85fc 128 LineReverseFlag=0;
lilac0112_1 0:ea35c18c85fc 129
lilac0112_1 0:ea35c18c85fc 130 LineSign[A_SPOT]=0;
lilac0112_1 0:ea35c18c85fc 131 LineSign[B_SPOT]=0;
lilac0112_1 0:ea35c18c85fc 132 LineSign[C_SPOT]=0;
lilac0112_1 0:ea35c18c85fc 133
lilac0112_1 2:635947de1583 134 data.lnFlag[A_SPOT]=0;
lilac0112_1 2:635947de1583 135 data.lnFlag[B_SPOT]=0;
lilac0112_1 2:635947de1583 136 data.lnFlag[C_SPOT]=0;
lilac0112_1 0:ea35c18c85fc 137
lilac0112_1 0:ea35c18c85fc 138 sys.KickOffFlag=0;
lilac0112_1 0:ea35c18c85fc 139 }
lilac0112_1 0:ea35c18c85fc 140 if(sys.IrFlag==1){
lilac0112_1 0:ea35c18c85fc 141 ReadIr();
lilac0112_1 0:ea35c18c85fc 142 sys.IrFlag=0;
lilac0112_1 0:ea35c18c85fc 143 }
lilac0112_1 0:ea35c18c85fc 144 if(sys.PidFlag==1){
lilac0112_1 0:ea35c18c85fc 145 //Line_ticker.detach();
lilac0112_1 0:ea35c18c85fc 146 PidUpdate();
lilac0112_1 0:ea35c18c85fc 147 //Line_ticker.attach(&ReadLine, 0.005);
lilac0112_1 0:ea35c18c85fc 148 sys.PidFlag=0;
lilac0112_1 0:ea35c18c85fc 149 }
lilac0112_1 5:5ff3a7d5d8c2 150 ir_x = ir_move_val_old[data.irNotice][data.irPosition][IR_X];
lilac0112_1 5:5ff3a7d5d8c2 151 ir_y = ir_move_val_old[data.irNotice][data.irPosition][IR_Y];
lilac0112_1 2:635947de1583 152 if(data.irPosition<8){
lilac0112_1 0:ea35c18c85fc 153 ir_x *= sys.l_pow;
lilac0112_1 0:ea35c18c85fc 154 ir_y *= sys.l_pow;
lilac0112_1 0:ea35c18c85fc 155 }
lilac0112_1 0:ea35c18c85fc 156 else{
lilac0112_1 0:ea35c18c85fc 157 ir_x *= sys.s_pow;
lilac0112_1 0:ea35c18c85fc 158 ir_y *= sys.s_pow;
lilac0112_1 0:ea35c18c85fc 159 }
lilac0112_1 0:ea35c18c85fc 160
lilac0112_1 0:ea35c18c85fc 161 //Lineを考慮していないIrのみの値
lilac0112_1 0:ea35c18c85fc 162 vx = ir_x;
lilac0112_1 0:ea35c18c85fc 163 vy = ir_y;
lilac0112_1 0:ea35c18c85fc 164 //Line検出方向を調べる
lilac0112_1 2:635947de1583 165 LineDir[A_SPOT] = (!((vx>0)&&((data.lnFlag[A_SPOT]==1)&&(data.lnFlag[B_SPOT]==0))&&(1)));
lilac0112_1 2:635947de1583 166 LineDir[B_SPOT] = (!((vx<0)&&((data.lnFlag[A_SPOT]==0)&&(data.lnFlag[B_SPOT]==1))&&(1)));
lilac0112_1 2:635947de1583 167 LineDir[C_SPOT] = (!((vy<0)&&((data.lnFlag[C_SPOT]==1)&&(1 ))&&(1)));
lilac0112_1 2:635947de1583 168 LineDir[AB_SPOT] = (!((vy>0)&&((data.lnFlag[A_SPOT]==1)&&(data.lnFlag[B_SPOT]==1))&&(1)));
lilac0112_1 0:ea35c18c85fc 169
lilac0112_1 0:ea35c18c85fc 170 LineStop[X_AXIS] = LineDir[A_SPOT]*LineDir[B_SPOT];
lilac0112_1 0:ea35c18c85fc 171 LineStop[Y_AXIS] = LineDir[C_SPOT]*LineDir[AB_SPOT];
lilac0112_1 0:ea35c18c85fc 172
lilac0112_1 0:ea35c18c85fc 173 //Ir
lilac0112_1 0:ea35c18c85fc 174 //strict
lilac0112_1 0:ea35c18c85fc 175 /*
lilac0112_1 2:635947de1583 176 IrRange[A_SPOT] = ((18<=data.irPosition)&&(data.irPosition<=19))||((8<=data.irPosition)&&(data.irPosition<=10))
lilac0112_1 2:635947de1583 177 ||((6<=data.irPosition)&&(data.irPosition<=7))||(( 1)&&(data.irPosition<=1));
lilac0112_1 2:635947de1583 178 IrRange[B_SPOT] = ((12<=data.irPosition)&&(data.irPosition<=16))||((2<=data.irPosition)&&(data.irPosition<=5));
lilac0112_1 2:635947de1583 179 IrRange[C_SPOT] = ((15<=data.irPosition)&&(data.irPosition<=19))||((4<=data.irPosition)&&(data.irPosition<=7));
lilac0112_1 2:635947de1583 180 IrRange[AB_SPOT] = ((9<=data.irPosition)&&(data.irPosition<=13))||(( 1)&&(data.irPosition<=3));
lilac0112_1 0:ea35c18c85fc 181 */
lilac0112_1 0:ea35c18c85fc 182 //sweet
lilac0112_1 2:635947de1583 183 IrRange[A_SPOT] = ((19<=data.irPosition)&&(data.irPosition<=19))||((8<=data.irPosition)&&(data.irPosition<=9))
lilac0112_1 2:635947de1583 184 ||((6<=data.irPosition)&&(data.irPosition<=7))||(( 1)&&(data.irPosition<=1));
lilac0112_1 2:635947de1583 185 IrRange[B_SPOT] = ((13<=data.irPosition)&&(data.irPosition<=15))||((2<=data.irPosition)&&(data.irPosition<=5));
lilac0112_1 2:635947de1583 186 IrRange[C_SPOT] = ((16<=data.irPosition)&&(data.irPosition<=18))||((4<=data.irPosition)&&(data.irPosition<=7));
lilac0112_1 2:635947de1583 187 IrRange[AB_SPOT] = ((10<=data.irPosition)&&(data.irPosition<=12))||(( 1)&&(data.irPosition<=3));
lilac0112_1 0:ea35c18c85fc 188
lilac0112_1 0:ea35c18c85fc 189
lilac0112_1 0:ea35c18c85fc 190 LinePulse[A_SPOT] = ((IrRange[A_SPOT]==1)&&(LineDir[A_SPOT]==0));
lilac0112_1 0:ea35c18c85fc 191 LinePulse[B_SPOT] = ((IrRange[B_SPOT]==1)&&(LineDir[B_SPOT]==0));
lilac0112_1 0:ea35c18c85fc 192 LinePulse[C_SPOT] = ((IrRange[C_SPOT]==1)&&(LineDir[C_SPOT]==0));
lilac0112_1 0:ea35c18c85fc 193 LinePulse[AB_SPOT] = ((IrRange[AB_SPOT]==1)&&(LineDir[AB_SPOT]==0));
lilac0112_1 0:ea35c18c85fc 194
lilac0112_1 0:ea35c18c85fc 195 LineBind[A_SPOT] = ((LinePulse[A_SPOT])||((IrRange[A_SPOT]==1)&&(LineBind[A_SPOT]==1)))&&(LineBind[B_SPOT]==0);
lilac0112_1 0:ea35c18c85fc 196 LineBind[B_SPOT] = ((LinePulse[B_SPOT])||((IrRange[B_SPOT]==1)&&(LineBind[B_SPOT]==1)))&&(LineBind[A_SPOT]==0);
lilac0112_1 0:ea35c18c85fc 197 LineBind[C_SPOT] = ((LinePulse[C_SPOT])||((IrRange[C_SPOT]==1)&&(LineBind[C_SPOT]==1)))&&(LineBind[AB_SPOT]==0);
lilac0112_1 0:ea35c18c85fc 198 LineBind[AB_SPOT] = ((LinePulse[AB_SPOT])||((IrRange[AB_SPOT]==1)&&(LineBind[AB_SPOT]==1)))&&(LineBind[C_SPOT]==0);
lilac0112_1 0:ea35c18c85fc 199 /*
lilac0112_1 0:ea35c18c85fc 200 LineBind[A_SPOT] = ((IrRange[A_SPOT]==1)&&((LineDir[A_SPOT]==0)||(LineBind[A_SPOT]==1)));
lilac0112_1 0:ea35c18c85fc 201 LineBind[B_SPOT] = ((IrRange[B_SPOT]==1)&&((LineDir[B_SPOT]==0)||(LineBind[B_SPOT]==1)));
lilac0112_1 0:ea35c18c85fc 202 LineBind[C_SPOT] = ((IrRange[C_SPOT]==1)&&((LineDir[C_SPOT]==0)||(LineBind[C_SPOT]==1)));
lilac0112_1 0:ea35c18c85fc 203 LineBind[AB_SPOT] = ((IrRange[AB_SPOT]==1)&&((LineDir[AB_SPOT]==0)||(LineBind[AB_SPOT]==1)));
lilac0112_1 0:ea35c18c85fc 204 */
lilac0112_1 0:ea35c18c85fc 205 vx = vx*LineStop[X_AXIS] + (LINE_RF)*(vy!=0)*((-1)*(LineDir[A_SPOT]==0) + (LineDir[B_SPOT]==0));
lilac0112_1 0:ea35c18c85fc 206 vy = vy*LineStop[Y_AXIS] + (LINE_RF)*(vx!=0)*((LineDir[C_SPOT]==0) + (-1)*(LineDir[AB_SPOT]==0));
lilac0112_1 0:ea35c18c85fc 207 vs = cmps_set.OutputPID;
lilac0112_1 0:ea35c18c85fc 208 if((LineBind[A_SPOT]==1)||(LineBind[B_SPOT]==1)||(LineBind[C_SPOT]==1)||(LineBind[AB_SPOT]==1)){
lilac0112_1 2:635947de1583 209 if(LineRaw>0){
lilac0112_1 0:ea35c18c85fc 210 vx=(LINE_RF*2)*((-1)*IrRange[A_SPOT] + IrRange[B_SPOT]);
lilac0112_1 0:ea35c18c85fc 211 vy=(LINE_RF*2)*( IrRange[C_SPOT] + (-1)*IrRange[AB_SPOT]);
lilac0112_1 0:ea35c18c85fc 212 /*vx=(LINE_RF*2)*((-1)*(LineFirst[X_AXIS] == A_SPOT) + (LineFirst[X_AXIS] == B_SPOT));
lilac0112_1 0:ea35c18c85fc 213 vy=(LINE_RF*2)*( (LineFirst[Y_AXIS] == C_SPOT) + (-1)*(LineFirst[Y_AXIS] == AB_SPOT));
lilac0112_1 0:ea35c18c85fc 214 Line_timeout[A_SPOT].attach(&LineClear_A, LINE_DELAY);
lilac0112_1 0:ea35c18c85fc 215 Line_timeout[B_SPOT].attach(&LineClear_B, LINE_DELAY);
lilac0112_1 0:ea35c18c85fc 216 Line_timeout[C_SPOT].attach(&LineClear_C, LINE_DELAY);*/
lilac0112_1 0:ea35c18c85fc 217 }
lilac0112_1 0:ea35c18c85fc 218 else{
lilac0112_1 0:ea35c18c85fc 219 vx=0;
lilac0112_1 0:ea35c18c85fc 220 vy=0;
lilac0112_1 0:ea35c18c85fc 221 }
lilac0112_1 0:ea35c18c85fc 222 }
lilac0112_1 2:635947de1583 223 if(LineRaw>0){
lilac0112_1 0:ea35c18c85fc 224 Line_timeout[A_SPOT].attach(&LineClear_A, LINE_DELAY);
lilac0112_1 0:ea35c18c85fc 225 Line_timeout[B_SPOT].attach(&LineClear_B, LINE_DELAY);
lilac0112_1 0:ea35c18c85fc 226 Line_timeout[C_SPOT].attach(&LineClear_C, LINE_DELAY);
lilac0112_1 0:ea35c18c85fc 227 }
lilac0112_1 0:ea35c18c85fc 228 move(
lilac0112_1 0:ea35c18c85fc 229 vx,
lilac0112_1 0:ea35c18c85fc 230 vy,
lilac0112_1 0:ea35c18c85fc 231 vs
lilac0112_1 0:ea35c18c85fc 232 );
lilac0112_1 0:ea35c18c85fc 233 if(sys.MotorFlag==1){
lilac0112_1 0:ea35c18c85fc 234 tx_motor();
lilac0112_1 0:ea35c18c85fc 235 sys.MotorFlag=0;
lilac0112_1 0:ea35c18c85fc 236 }
lilac0112_1 0:ea35c18c85fc 237 return;
lilac0112_1 0:ea35c18c85fc 238 }
lilac0112_1 0:ea35c18c85fc 239 void modeAttack2(void){
lilac0112_1 0:ea35c18c85fc 240 double ir_x, ir_y;
lilac0112_1 5:5ff3a7d5d8c2 241 int vx,vy,vs;
lilac0112_1 5:5ff3a7d5d8c2 242 /*int LineForce[2];
lilac0112_1 0:ea35c18c85fc 243 uint8_t LineDir[4];
lilac0112_1 0:ea35c18c85fc 244 uint8_t LineOn[4];
lilac0112_1 0:ea35c18c85fc 245 uint8_t LineReturn[4];
lilac0112_1 0:ea35c18c85fc 246 uint8_t LineStop[2];
lilac0112_1 0:ea35c18c85fc 247 uint8_t IrRange[4];
lilac0112_1 5:5ff3a7d5d8c2 248 uint8_t static LineBind[4];*/
lilac0112_1 0:ea35c18c85fc 249 //buint8_t static spi_count;
lilac0112_1 0:ea35c18c85fc 250 if(sys.KickOffFlag==1){
lilac0112_1 5:5ff3a7d5d8c2 251
lilac0112_1 5:5ff3a7d5d8c2 252 sys.MotorFlag=0;
lilac0112_1 5:5ff3a7d5d8c2 253 sys.IrFlag=0;
lilac0112_1 5:5ff3a7d5d8c2 254 /*LineBind[0]=0;
lilac0112_1 0:ea35c18c85fc 255 LineBind[1]=0;
lilac0112_1 0:ea35c18c85fc 256 LineBind[2]=0;
lilac0112_1 0:ea35c18c85fc 257 LineBind[3]=0;
lilac0112_1 0:ea35c18c85fc 258 LineReverseFlag=0;
lilac0112_1 0:ea35c18c85fc 259
lilac0112_1 0:ea35c18c85fc 260 LineSign[A_SPOT]=0;
lilac0112_1 0:ea35c18c85fc 261 LineSign[B_SPOT]=0;
lilac0112_1 0:ea35c18c85fc 262 LineSign[C_SPOT]=0;
lilac0112_1 0:ea35c18c85fc 263
lilac0112_1 2:635947de1583 264 data.lnFlag[A_SPOT]=0;
lilac0112_1 2:635947de1583 265 data.lnFlag[B_SPOT]=0;
lilac0112_1 2:635947de1583 266 data.lnFlag[C_SPOT]=0;
lilac0112_1 0:ea35c18c85fc 267
lilac0112_1 0:ea35c18c85fc 268 HmcResetFlag = 0;
lilac0112_1 10:6df631c39f9b 269 sys.UswFlag = 0;
lilac0112_1 0:ea35c18c85fc 270 //spi_count=0;
lilac0112_1 5:5ff3a7d5d8c2 271 */
lilac0112_1 10:6df631c39f9b 272 hmc_reset=HMC_RUN;
lilac0112_1 0:ea35c18c85fc 273 sys.KickFlag = 0;
lilac0112_1 0:ea35c18c85fc 274
lilac0112_1 0:ea35c18c85fc 275 sys.KickOffFlag=0;
lilac0112_1 0:ea35c18c85fc 276 //while((Sw[2].read()==1)&&(Sw[3].read()==1));//押して離すとスタート
lilac0112_1 0:ea35c18c85fc 277 }
lilac0112_1 0:ea35c18c85fc 278 if(sys.IrFlag==1){
lilac0112_1 5:5ff3a7d5d8c2 279 //LED[0] = 0;
lilac0112_1 5:5ff3a7d5d8c2 280 //LED[1] = 1;
lilac0112_1 0:ea35c18c85fc 281 /*spi_count++;
lilac0112_1 0:ea35c18c85fc 282 if(spi_count%10 == 0){
lilac0112_1 0:ea35c18c85fc 283 ReadPing();
lilac0112_1 0:ea35c18c85fc 284 }
lilac0112_1 0:ea35c18c85fc 285 else{
lilac0112_1 0:ea35c18c85fc 286 ReadIr();
lilac0112_1 0:ea35c18c85fc 287 }
lilac0112_1 0:ea35c18c85fc 288 if(spi_count==20) spi_count=0;
lilac0112_1 0:ea35c18c85fc 289 */
lilac0112_1 0:ea35c18c85fc 290 ReadIr();
lilac0112_1 0:ea35c18c85fc 291 sys.IrFlag=0;
lilac0112_1 0:ea35c18c85fc 292 }
lilac0112_1 0:ea35c18c85fc 293 if(sys.PidFlag==1){
lilac0112_1 0:ea35c18c85fc 294 PidUpdate();
lilac0112_1 0:ea35c18c85fc 295 sys.PidFlag=0;
lilac0112_1 0:ea35c18c85fc 296 }
lilac0112_1 10:6df631c39f9b 297 /*if(sys.UswFlag==1){
lilac0112_1 0:ea35c18c85fc 298 ReadPing();
lilac0112_1 10:6df631c39f9b 299 sys.UswFlag=0;
lilac0112_1 5:5ff3a7d5d8c2 300 }*/
lilac0112_1 0:ea35c18c85fc 301 /*
lilac0112_1 0:ea35c18c85fc 302 if(HmcResetFlag==1){
lilac0112_1 0:ea35c18c85fc 303 HmcReset();
lilac0112_1 0:ea35c18c85fc 304 HmcResetFlag=0;
lilac0112_1 0:ea35c18c85fc 305 }
lilac0112_1 0:ea35c18c85fc 306 */
lilac0112_1 5:5ff3a7d5d8c2 307 //ir_x = ir_move_val_old[data.irNotice][data.irPosition][IR_X];
lilac0112_1 5:5ff3a7d5d8c2 308 //ir_y = ir_move_val_old[data.irNotice][data.irPosition][IR_Y];
lilac0112_1 5:5ff3a7d5d8c2 309 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 310 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 311
lilac0112_1 2:635947de1583 312 if(data.irPosition<8){
lilac0112_1 0:ea35c18c85fc 313 ir_x *= sys.l_pow;
lilac0112_1 0:ea35c18c85fc 314 ir_y *= sys.l_pow;
lilac0112_1 0:ea35c18c85fc 315 }
lilac0112_1 0:ea35c18c85fc 316 else{
lilac0112_1 0:ea35c18c85fc 317 ir_x *= sys.s_pow;
lilac0112_1 0:ea35c18c85fc 318 ir_y *= sys.s_pow;
lilac0112_1 0:ea35c18c85fc 319 }
lilac0112_1 0:ea35c18c85fc 320
lilac0112_1 0:ea35c18c85fc 321 //Lineを考慮していないIrのみの値
lilac0112_1 0:ea35c18c85fc 322 vx = ir_x;
lilac0112_1 0:ea35c18c85fc 323 vy = ir_y;
lilac0112_1 5:5ff3a7d5d8c2 324 /*
lilac0112_1 2:635947de1583 325 if((data.irPosition==10)&&(vy>0)){
lilac0112_1 0:ea35c18c85fc 326 vy += 0;//前進加速
lilac0112_1 0:ea35c18c85fc 327 }
lilac0112_1 2:635947de1583 328 if((data.irPosition==11)&&(vy>0)){
lilac0112_1 0:ea35c18c85fc 329 vy += 0;//前進加速
lilac0112_1 0:ea35c18c85fc 330 if(sys.KickFlag==1){
lilac0112_1 0:ea35c18c85fc 331 DriveSolenoid();
lilac0112_1 0:ea35c18c85fc 332 }
lilac0112_1 0:ea35c18c85fc 333 }
lilac0112_1 2:635947de1583 334 if((data.irPosition==12)&&(vy>0)){
lilac0112_1 0:ea35c18c85fc 335 vy += 0;//前進加速
lilac0112_1 0:ea35c18c85fc 336 }
lilac0112_1 2:635947de1583 337 if((data.irPosition==1)&&(vy>0)){
lilac0112_1 0:ea35c18c85fc 338 vy += 0;//前進加速
lilac0112_1 0:ea35c18c85fc 339 }
lilac0112_1 2:635947de1583 340 if((data.irPosition==2)&&(vy>0)){
lilac0112_1 0:ea35c18c85fc 341 vy += 0;//前進加速
lilac0112_1 0:ea35c18c85fc 342 }
lilac0112_1 0:ea35c18c85fc 343
lilac0112_1 2:635947de1583 344 if((data.irPosition==17)&&(data.ping[L_PING]>data.ping[R_PING])){
lilac0112_1 0:ea35c18c85fc 345 vx *= -1.0;//背後回り込みの左右判断
lilac0112_1 5:5ff3a7d5d8c2 346 }*/
lilac0112_1 0:ea35c18c85fc 347 /*
lilac0112_1 0:ea35c18c85fc 348 if((cmps_set.InputPID<(REFERENCE-30))||(cmps_set.InputPID>(REFERENCE+30))){
lilac0112_1 0:ea35c18c85fc 349 vx = vx*(0.75);
lilac0112_1 0:ea35c18c85fc 350 vy = vy*(0.75);
lilac0112_1 0:ea35c18c85fc 351 }
lilac0112_1 0:ea35c18c85fc 352 */
lilac0112_1 0:ea35c18c85fc 353 //Lineを踏み始めた方向を調べる
lilac0112_1 2:635947de1583 354 /*LineDir[A_SPOT] = (vx>0)&&((data.lnFlag[A_SPOT]==1)&&(1 ))&&(LineFirst[X_AXIS] == A_SPOT);
lilac0112_1 2:635947de1583 355 LineDir[B_SPOT] = (vx<0)&&((data.lnFlag[B_SPOT]==1)&&(1 ))&&(LineFirst[X_AXIS] == B_SPOT);
lilac0112_1 2:635947de1583 356 LineDir[C_SPOT] = (vy<0)&&((data.lnFlag[C_SPOT]==1)&&(1 ))&&(LineFirst[Y_AXIS] == C_SPOT);
lilac0112_1 2:635947de1583 357 LineDir[AB_SPOT]= (vy>0)&&((data.lnFlag[A_SPOT]==1)&&(data.lnFlag[B_SPOT]==1))&&(LineFirst[Y_AXIS] == AB_SPOT);*/
lilac0112_1 5:5ff3a7d5d8c2 358 /*
lilac0112_1 0:ea35c18c85fc 359 LineDir[A_SPOT] = (vx>0)&&((LineSign[A_SPOT]==1)&&(1 ))&&(LineFirst[X_AXIS] == A_SPOT);
lilac0112_1 0:ea35c18c85fc 360 LineDir[B_SPOT] = (vx<0)&&((LineSign[B_SPOT]==1)&&(1 ))&&(LineFirst[X_AXIS] == B_SPOT);
lilac0112_1 0:ea35c18c85fc 361 LineDir[C_SPOT] = (vy<0)&&((LineSign[C_SPOT]==1)&&(1 ))&&(LineFirst[Y_AXIS] == C_SPOT);
lilac0112_1 0:ea35c18c85fc 362 LineDir[AB_SPOT]= (vy>0)&&((LineSign[A_SPOT]==1)&&(LineSign[B_SPOT]==1))&&(LineFirst[Y_AXIS] == AB_SPOT);
lilac0112_1 0:ea35c18c85fc 363
lilac0112_1 0:ea35c18c85fc 364 //Irボールの方向
lilac0112_1 0:ea35c18c85fc 365 //strict
lilac0112_1 0:ea35c18c85fc 366
lilac0112_1 2:635947de1583 367 IrRange[A_SPOT] = ((18<=data.irPosition)&&(data.irPosition<=19))||((8<=data.irPosition)&&(data.irPosition<=10))
lilac0112_1 2:635947de1583 368 ||((6<=data.irPosition)&&(data.irPosition<=7))||(( 1)&&(data.irPosition<=1));
lilac0112_1 2:635947de1583 369 IrRange[B_SPOT] = ((12<=data.irPosition)&&(data.irPosition<=16))||((2<=data.irPosition)&&(data.irPosition<=5));
lilac0112_1 2:635947de1583 370 IrRange[C_SPOT] = ((15<=data.irPosition)&&(data.irPosition<=19))||((4<=data.irPosition)&&(data.irPosition<=7));
lilac0112_1 2:635947de1583 371 IrRange[AB_SPOT] = ((9<=data.irPosition)&&(data.irPosition<=13))||(( 1)&&(data.irPosition<=3));
lilac0112_1 5:5ff3a7d5d8c2 372 */
lilac0112_1 0:ea35c18c85fc 373 //sweet
lilac0112_1 2:635947de1583 374 /*IrRange[A_SPOT] = ((19<=data.irPosition)&&(data.irPosition<=19))||((8<=data.irPosition)&&(data.irPosition<=9))
lilac0112_1 2:635947de1583 375 ||((6<=data.irPosition)&&(data.irPosition<=7))||(( 1)&&(data.irPosition<=1));
lilac0112_1 2:635947de1583 376 IrRange[B_SPOT] = ((13<=data.irPosition)&&(data.irPosition<=15))||((2<=data.irPosition)&&(data.irPosition<=5));
lilac0112_1 2:635947de1583 377 IrRange[C_SPOT] = ((16<=data.irPosition)&&(data.irPosition<=18))||((4<=data.irPosition)&&(data.irPosition<=7));
lilac0112_1 2:635947de1583 378 IrRange[AB_SPOT] = ((10<=data.irPosition)&&(data.irPosition<=12))||(( 1)&&(data.irPosition<=3));*/
lilac0112_1 5:5ff3a7d5d8c2 379 /*
lilac0112_1 0:ea35c18c85fc 380 //none
lilac0112_1 2:635947de1583 381 if(data.irNotice==IR_NONE){
lilac0112_1 0:ea35c18c85fc 382 IrRange[A_SPOT] = 0;
lilac0112_1 0:ea35c18c85fc 383 IrRange[B_SPOT] = 0;
lilac0112_1 0:ea35c18c85fc 384 IrRange[C_SPOT] = 0;
lilac0112_1 0:ea35c18c85fc 385 IrRange[AB_SPOT] = 0;
lilac0112_1 0:ea35c18c85fc 386 }
lilac0112_1 0:ea35c18c85fc 387 //白線を踏み始めた方向とボールの方向が一致．(SelfHold)
lilac0112_1 0:ea35c18c85fc 388 LineBind[A_SPOT] = (IrRange[A_SPOT]==1)&&((LineDir[A_SPOT]==1)||(LineBind[A_SPOT]==1));
lilac0112_1 0:ea35c18c85fc 389 LineBind[B_SPOT] = (IrRange[B_SPOT]==1)&&((LineDir[B_SPOT]==1)||(LineBind[B_SPOT]==1));
lilac0112_1 0:ea35c18c85fc 390 LineBind[C_SPOT] = (IrRange[C_SPOT]==1)&&((LineDir[C_SPOT]==1)||(LineBind[C_SPOT]==1));
lilac0112_1 0:ea35c18c85fc 391 LineBind[AB_SPOT] = (IrRange[AB_SPOT]==1)&&((LineDir[AB_SPOT]==1)||(LineBind[AB_SPOT]==1));
lilac0112_1 0:ea35c18c85fc 392
lilac0112_1 0:ea35c18c85fc 393 LineStop[X_AXIS] = (LineBind[A_SPOT]==0)*(LineBind[B_SPOT]==0);
lilac0112_1 0:ea35c18c85fc 394 LineStop[Y_AXIS] = (LineBind[C_SPOT]==0)*(LineBind[AB_SPOT]==0);
lilac0112_1 0:ea35c18c85fc 395
lilac0112_1 0:ea35c18c85fc 396 //白線踏んでる
lilac0112_1 2:635947de1583 397 if(LineRaw>0){
lilac0112_1 0:ea35c18c85fc 398 LineOn[A_SPOT] = (LineSign[A_SPOT]==1) &&(LineFirst[X_AXIS]==A_SPOT);
lilac0112_1 0:ea35c18c85fc 399 LineOn[B_SPOT] = (LineSign[B_SPOT]==1) &&(LineFirst[X_AXIS]==B_SPOT);
lilac0112_1 0:ea35c18c85fc 400 LineOn[C_SPOT] = (LineSign[C_SPOT]==1) &&(LineFirst[Y_AXIS]==C_SPOT);
lilac0112_1 0:ea35c18c85fc 401 LineOn[AB_SPOT] = ((LineSign[A_SPOT]==1)&&(LineSign[B_SPOT]==1))&&(LineFirst[Y_AXIS]==AB_SPOT);
lilac0112_1 0:ea35c18c85fc 402 //外側に向かう力を消す．
lilac0112_1 0:ea35c18c85fc 403 //x
lilac0112_1 0:ea35c18c85fc 404 if(((LineOn[A_SPOT]==1)&&(vx>0))||((LineOn[B_SPOT]==1)&&(vx<0))){
lilac0112_1 0:ea35c18c85fc 405 vx=0;
lilac0112_1 0:ea35c18c85fc 406 //yの力を加える．
lilac0112_1 0:ea35c18c85fc 407 if(vy>0){vy += 10;}
lilac0112_1 0:ea35c18c85fc 408 if(vy<0){vy -= 10;}
lilac0112_1 0:ea35c18c85fc 409 }
lilac0112_1 0:ea35c18c85fc 410 //y
lilac0112_1 0:ea35c18c85fc 411 if(((LineOn[C_SPOT]==1)&&(vy<0))||((LineOn[AB_SPOT]==1)&&(vy>0))){
lilac0112_1 0:ea35c18c85fc 412 vy=0;
lilac0112_1 0:ea35c18c85fc 413 }
lilac0112_1 0:ea35c18c85fc 414 //内側に向かう力を加える．
lilac0112_1 0:ea35c18c85fc 415 LineReturn[A_SPOT] = (LineOn[A_SPOT]==1);
lilac0112_1 0:ea35c18c85fc 416 if((LineReturn[A_SPOT]==1)&&(LineOn[AB_SPOT]==1)){
lilac0112_1 0:ea35c18c85fc 417 if(LineOn[C_SPOT]==0){
lilac0112_1 0:ea35c18c85fc 418 LineReturn[A_SPOT]=0;
lilac0112_1 0:ea35c18c85fc 419 }
lilac0112_1 0:ea35c18c85fc 420 else{
lilac0112_1 0:ea35c18c85fc 421 LineReturn[A_SPOT]=1;
lilac0112_1 0:ea35c18c85fc 422 }
lilac0112_1 0:ea35c18c85fc 423 }
lilac0112_1 0:ea35c18c85fc 424
lilac0112_1 0:ea35c18c85fc 425 LineReturn[B_SPOT] = (LineOn[B_SPOT]==1);
lilac0112_1 0:ea35c18c85fc 426 if((LineReturn[B_SPOT]==1)&&(LineOn[AB_SPOT]==1)){
lilac0112_1 0:ea35c18c85fc 427 if(LineOn[C_SPOT]==0){
lilac0112_1 0:ea35c18c85fc 428 LineReturn[B_SPOT]=0;
lilac0112_1 0:ea35c18c85fc 429 }
lilac0112_1 0:ea35c18c85fc 430 else{
lilac0112_1 0:ea35c18c85fc 431 LineReturn[B_SPOT]=1;
lilac0112_1 0:ea35c18c85fc 432 }
lilac0112_1 0:ea35c18c85fc 433 }
lilac0112_1 0:ea35c18c85fc 434 LineReturn[C_SPOT] = (LineOn[C_SPOT]==1);
lilac0112_1 0:ea35c18c85fc 435 if(LineReturn[C_SPOT]==1){
lilac0112_1 0:ea35c18c85fc 436 LineReturn[A_SPOT]=0;
lilac0112_1 0:ea35c18c85fc 437 LineReturn[B_SPOT]=0;
lilac0112_1 0:ea35c18c85fc 438 }
lilac0112_1 0:ea35c18c85fc 439 LineReturn[AB_SPOT] = (LineOn[AB_SPOT]==1);
lilac0112_1 0:ea35c18c85fc 440
lilac0112_1 0:ea35c18c85fc 441 LineForce[X_AXIS] = (LINE_RF*2)*((-1)*(LineReturn[A_SPOT]==1) + ( 1)*(LineReturn[B_SPOT]==1)) +
lilac0112_1 0:ea35c18c85fc 442 (LINE_RF*2)*(LineReturn[AB_SPOT]==1)*(LineReturn[A_SPOT]==0)*(LineReturn[B_SPOT]==0)
lilac0112_1 2:635947de1583 443 *(( 1)*(data.ping[L_PING]<40) + (-1)*(data.ping[R_PING]<40)) +
lilac0112_1 0:ea35c18c85fc 444 (LINE_RF*2)*(LineReturn[C_SPOT]==1)*(LineReturn[A_SPOT]==0)*(LineReturn[B_SPOT]==0)
lilac0112_1 2:635947de1583 445 *(( 1)*(data.ping[L_PING]<40) + (-1)*(data.ping[R_PING]<40));
lilac0112_1 0:ea35c18c85fc 446 LineForce[Y_AXIS] = (LINE_RF*2)*(( 1)*(LineReturn[C_SPOT]==1) + (-1)*(LineReturn[AB_SPOT]==1));
lilac0112_1 0:ea35c18c85fc 447
lilac0112_1 0:ea35c18c85fc 448 Line_timeout[A_SPOT].attach(&LineClear_A, LINE_DELAY);
lilac0112_1 0:ea35c18c85fc 449 Line_timeout[B_SPOT].attach(&LineClear_B, LINE_DELAY);
lilac0112_1 0:ea35c18c85fc 450 Line_timeout[C_SPOT].attach(&LineClear_C, LINE_DELAY);
lilac0112_1 0:ea35c18c85fc 451 }
lilac0112_1 0:ea35c18c85fc 452 else{
lilac0112_1 0:ea35c18c85fc 453 LineForce[X_AXIS] = 0;
lilac0112_1 0:ea35c18c85fc 454 LineForce[Y_AXIS] = 0;
lilac0112_1 0:ea35c18c85fc 455 }
lilac0112_1 5:5ff3a7d5d8c2 456 */
lilac0112_1 5:5ff3a7d5d8c2 457 //vx = vx*LineStop[X_AXIS] + LineForce[X_AXIS];
lilac0112_1 5:5ff3a7d5d8c2 458 //vy = vy*LineStop[Y_AXIS] + LineForce[Y_AXIS];
lilac0112_1 0:ea35c18c85fc 459 vs = cmps_set.OutputPID;
lilac0112_1 0:ea35c18c85fc 460 move(
lilac0112_1 0:ea35c18c85fc 461 vx,
lilac0112_1 0:ea35c18c85fc 462 vy,
lilac0112_1 0:ea35c18c85fc 463 vs
lilac0112_1 0:ea35c18c85fc 464 );
lilac0112_1 0:ea35c18c85fc 465 /*move(
lilac0112_1 0:ea35c18c85fc 466 0,
lilac0112_1 0:ea35c18c85fc 467 0,
lilac0112_1 0:ea35c18c85fc 468 10
lilac0112_1 0:ea35c18c85fc 469 );*/
lilac0112_1 0:ea35c18c85fc 470 if(sys.MotorFlag==1){
lilac0112_1 0:ea35c18c85fc 471 tx_motor();
lilac0112_1 0:ea35c18c85fc 472 sys.MotorFlag=0;
lilac0112_1 0:ea35c18c85fc 473 }
lilac0112_1 0:ea35c18c85fc 474 return;
lilac0112_1 0:ea35c18c85fc 475 }
lilac0112_1 0:ea35c18c85fc 476 void modeAttack3(void){
lilac0112_1 0:ea35c18c85fc 477 double ir_x, ir_y;
lilac0112_1 0:ea35c18c85fc 478 int vx,vy,vs, LineForce[2];
lilac0112_1 0:ea35c18c85fc 479 uint8_t LineDir[4];
lilac0112_1 0:ea35c18c85fc 480 uint8_t LineOn[4];
lilac0112_1 0:ea35c18c85fc 481 uint8_t LineReturn[4];
lilac0112_1 0:ea35c18c85fc 482 uint8_t LineStop[2];
lilac0112_1 0:ea35c18c85fc 483 uint8_t IrRange[4];
lilac0112_1 0:ea35c18c85fc 484 uint8_t static LineBind[4];
lilac0112_1 0:ea35c18c85fc 485 //buint8_t static spi_count;
lilac0112_1 0:ea35c18c85fc 486 if(sys.KickOffFlag==1){
lilac0112_1 0:ea35c18c85fc 487 LineBind[0]=0;
lilac0112_1 0:ea35c18c85fc 488 LineBind[1]=0;
lilac0112_1 0:ea35c18c85fc 489 LineBind[2]=0;
lilac0112_1 0:ea35c18c85fc 490 LineBind[3]=0;
lilac0112_1 0:ea35c18c85fc 491 LineReverseFlag=0;
lilac0112_1 0:ea35c18c85fc 492
lilac0112_1 0:ea35c18c85fc 493 LineSign[A_SPOT]=0;
lilac0112_1 0:ea35c18c85fc 494 LineSign[B_SPOT]=0;
lilac0112_1 0:ea35c18c85fc 495 LineSign[C_SPOT]=0;
lilac0112_1 0:ea35c18c85fc 496
lilac0112_1 2:635947de1583 497 data.lnFlag[A_SPOT]=0;
lilac0112_1 2:635947de1583 498 data.lnFlag[B_SPOT]=0;
lilac0112_1 2:635947de1583 499 data.lnFlag[C_SPOT]=0;
lilac0112_1 0:ea35c18c85fc 500
lilac0112_1 0:ea35c18c85fc 501 LinePriority[X_AXIS]=0;
lilac0112_1 0:ea35c18c85fc 502 LinePriority[Y_AXIS]=0;
lilac0112_1 0:ea35c18c85fc 503
lilac0112_1 0:ea35c18c85fc 504 HmcResetFlag = 0;
lilac0112_1 10:6df631c39f9b 505 sys.UswFlag = 0;
lilac0112_1 0:ea35c18c85fc 506 //spi_count=0;
lilac0112_1 0:ea35c18c85fc 507
lilac0112_1 10:6df631c39f9b 508 hmc_reset=HMC_RUN;
lilac0112_1 0:ea35c18c85fc 509 sys.KickFlag = 0;
lilac0112_1 0:ea35c18c85fc 510
lilac0112_1 0:ea35c18c85fc 511 sys.KickOffFlag=0;
lilac0112_1 0:ea35c18c85fc 512 //while((Sw[2].read()==1)&&(Sw[3].read()==1));//押して離すとスタート
lilac0112_1 0:ea35c18c85fc 513 }
lilac0112_1 0:ea35c18c85fc 514 if(sys.IrFlag==1){
lilac0112_1 0:ea35c18c85fc 515 /*spi_count++;
lilac0112_1 0:ea35c18c85fc 516 if(spi_count%10 == 0){
lilac0112_1 0:ea35c18c85fc 517 ReadPing();
lilac0112_1 0:ea35c18c85fc 518 }
lilac0112_1 0:ea35c18c85fc 519 else{
lilac0112_1 0:ea35c18c85fc 520 ReadIr();
lilac0112_1 0:ea35c18c85fc 521 }
lilac0112_1 0:ea35c18c85fc 522 if(spi_count==20) spi_count=0;
lilac0112_1 0:ea35c18c85fc 523 */
lilac0112_1 0:ea35c18c85fc 524 ReadIr();
lilac0112_1 0:ea35c18c85fc 525 sys.IrFlag=0;
lilac0112_1 0:ea35c18c85fc 526 }
lilac0112_1 0:ea35c18c85fc 527 if(sys.PidFlag==1){
lilac0112_1 0:ea35c18c85fc 528 PidUpdate();
lilac0112_1 0:ea35c18c85fc 529 sys.PidFlag=0;
lilac0112_1 0:ea35c18c85fc 530 }
lilac0112_1 10:6df631c39f9b 531 if(sys.UswFlag==1){
lilac0112_1 0:ea35c18c85fc 532 ReadPing();
lilac0112_1 10:6df631c39f9b 533 sys.UswFlag=0;
lilac0112_1 0:ea35c18c85fc 534 }
lilac0112_1 0:ea35c18c85fc 535 /*
lilac0112_1 0:ea35c18c85fc 536 if(HmcResetFlag==1){
lilac0112_1 0:ea35c18c85fc 537 HmcReset();
lilac0112_1 0:ea35c18c85fc 538 HmcResetFlag=0;
lilac0112_1 0:ea35c18c85fc 539 }
lilac0112_1 0:ea35c18c85fc 540 */
lilac0112_1 5:5ff3a7d5d8c2 541 ir_x = ir_move_val_old[data.irNotice][data.irPosition][IR_X];
lilac0112_1 5:5ff3a7d5d8c2 542 ir_y = ir_move_val_old[data.irNotice][data.irPosition][IR_Y];
lilac0112_1 2:635947de1583 543 if(data.irPosition<8){
lilac0112_1 0:ea35c18c85fc 544 ir_x *= sys.l_pow;
lilac0112_1 0:ea35c18c85fc 545 ir_y *= sys.l_pow;
lilac0112_1 0:ea35c18c85fc 546 }
lilac0112_1 0:ea35c18c85fc 547 else{
lilac0112_1 0:ea35c18c85fc 548 ir_x *= sys.s_pow;
lilac0112_1 0:ea35c18c85fc 549 ir_y *= sys.s_pow;
lilac0112_1 0:ea35c18c85fc 550 }
lilac0112_1 0:ea35c18c85fc 551
lilac0112_1 0:ea35c18c85fc 552 //Lineを考慮していないIrのみの値
lilac0112_1 0:ea35c18c85fc 553 vx = ir_x;
lilac0112_1 0:ea35c18c85fc 554 vy = ir_y;
lilac0112_1 0:ea35c18c85fc 555
lilac0112_1 2:635947de1583 556 if((data.irPosition==10)&&(vy>0)){
lilac0112_1 0:ea35c18c85fc 557 vy += 15;//前進加速
lilac0112_1 0:ea35c18c85fc 558 }
lilac0112_1 2:635947de1583 559 if((data.irPosition==11)&&(vy>0)){
lilac0112_1 0:ea35c18c85fc 560 vy += 15;//前進加速
lilac0112_1 0:ea35c18c85fc 561 if(sys.KickFlag==1){
lilac0112_1 0:ea35c18c85fc 562 DriveSolenoid();
lilac0112_1 0:ea35c18c85fc 563 }
lilac0112_1 0:ea35c18c85fc 564 }
lilac0112_1 2:635947de1583 565 if((data.irPosition==12)&&(vy>0)){
lilac0112_1 0:ea35c18c85fc 566 vy += 15;//前進加速
lilac0112_1 0:ea35c18c85fc 567 }
lilac0112_1 2:635947de1583 568 if((data.irPosition==1)&&(vy>0)){
lilac0112_1 0:ea35c18c85fc 569 vy += 25;//前進加速
lilac0112_1 0:ea35c18c85fc 570 }
lilac0112_1 2:635947de1583 571 if((data.irPosition==2)&&(vy>0)){
lilac0112_1 0:ea35c18c85fc 572 vy += 25;//前進加速
lilac0112_1 0:ea35c18c85fc 573 }
lilac0112_1 0:ea35c18c85fc 574
lilac0112_1 0:ea35c18c85fc 575
lilac0112_1 2:635947de1583 576 if((data.irPosition==17)&&(data.ping[L_PING]>data.ping[R_PING])){
lilac0112_1 0:ea35c18c85fc 577 vx *= -1.0;//背後回り込みの左右判断
lilac0112_1 0:ea35c18c85fc 578 }
lilac0112_1 0:ea35c18c85fc 579 /*
lilac0112_1 0:ea35c18c85fc 580 if((cmps_set.InputPID<(REFERENCE-30))||(cmps_set.InputPID>(REFERENCE+30))){
lilac0112_1 0:ea35c18c85fc 581 vx = vx*(0.75);
lilac0112_1 0:ea35c18c85fc 582 vy = vy*(0.75);
lilac0112_1 0:ea35c18c85fc 583 }
lilac0112_1 0:ea35c18c85fc 584 */
lilac0112_1 0:ea35c18c85fc 585 //Lineを踏み始めた方向を調べる
lilac0112_1 2:635947de1583 586 /*LineDir[A_SPOT] = (vx>0)&&((data.lnFlag[A_SPOT]==1)&&(1 ))&&(LineFirst[X_AXIS] == A_SPOT);
lilac0112_1 2:635947de1583 587 LineDir[B_SPOT] = (vx<0)&&((data.lnFlag[B_SPOT]==1)&&(1 ))&&(LineFirst[X_AXIS] == B_SPOT);
lilac0112_1 2:635947de1583 588 LineDir[C_SPOT] = (vy<0)&&((data.lnFlag[C_SPOT]==1)&&(1 ))&&(LineFirst[Y_AXIS] == C_SPOT);
lilac0112_1 2:635947de1583 589 LineDir[AB_SPOT]= (vy>0)&&((data.lnFlag[A_SPOT]==1)&&(data.lnFlag[B_SPOT]==1))&&(LineFirst[Y_AXIS] == AB_SPOT);*/
lilac0112_1 0:ea35c18c85fc 590
lilac0112_1 0:ea35c18c85fc 591 LineDir[A_SPOT] = (vx>0)&&((LineSign[A_SPOT]==1)&&(1 ))&&(LineFirst[X_AXIS] == A_SPOT);
lilac0112_1 0:ea35c18c85fc 592 LineDir[B_SPOT] = (vx<0)&&((LineSign[B_SPOT]==1)&&(1 ))&&(LineFirst[X_AXIS] == B_SPOT);
lilac0112_1 0:ea35c18c85fc 593 LineDir[C_SPOT] = (vy<0)&&((LineSign[C_SPOT]==1)&&(1 ))&&(LineFirst[Y_AXIS] == C_SPOT);
lilac0112_1 0:ea35c18c85fc 594 LineDir[AB_SPOT]= (vy>0)&&((LineSign[A_SPOT]==1)&&(LineSign[B_SPOT]==1))&&(LineFirst[Y_AXIS] == AB_SPOT);
lilac0112_1 0:ea35c18c85fc 595
lilac0112_1 0:ea35c18c85fc 596 //Irボールの方向
lilac0112_1 0:ea35c18c85fc 597 //strict
lilac0112_1 0:ea35c18c85fc 598
lilac0112_1 2:635947de1583 599 IrRange[A_SPOT] = ((18<=data.irPosition)&&(data.irPosition<=19))||((8<=data.irPosition)&&(data.irPosition<=10))
lilac0112_1 2:635947de1583 600 ||((6<=data.irPosition)&&(data.irPosition<=7))||(( 1)&&(data.irPosition<=1));
lilac0112_1 2:635947de1583 601 IrRange[B_SPOT] = ((12<=data.irPosition)&&(data.irPosition<=16))||((2<=data.irPosition)&&(data.irPosition<=5));
lilac0112_1 2:635947de1583 602 IrRange[C_SPOT] = ((15<=data.irPosition)&&(data.irPosition<=19))||((4<=data.irPosition)&&(data.irPosition<=7));
lilac0112_1 2:635947de1583 603 IrRange[AB_SPOT] = ((9<=data.irPosition)&&(data.irPosition<=13))||(( 1)&&(data.irPosition<=3));
lilac0112_1 0:ea35c18c85fc 604
lilac0112_1 0:ea35c18c85fc 605 //sweet
lilac0112_1 2:635947de1583 606 /*IrRange[A_SPOT] = ((19<=data.irPosition)&&(data.irPosition<=19))||((8<=data.irPosition)&&(data.irPosition<=9))
lilac0112_1 2:635947de1583 607 ||((6<=data.irPosition)&&(data.irPosition<=7))||(( 1)&&(data.irPosition<=1));
lilac0112_1 2:635947de1583 608 IrRange[B_SPOT] = ((13<=data.irPosition)&&(data.irPosition<=15))||((2<=data.irPosition)&&(data.irPosition<=5));
lilac0112_1 2:635947de1583 609 IrRange[C_SPOT] = ((16<=data.irPosition)&&(data.irPosition<=18))||((4<=data.irPosition)&&(data.irPosition<=7));
lilac0112_1 2:635947de1583 610 IrRange[AB_SPOT] = ((10<=data.irPosition)&&(data.irPosition<=12))||(( 1)&&(data.irPosition<=3));*/
lilac0112_1 0:ea35c18c85fc 611 //none
lilac0112_1 2:635947de1583 612 if(data.irNotice==IR_NONE){
lilac0112_1 0:ea35c18c85fc 613 IrRange[A_SPOT] = 0;
lilac0112_1 0:ea35c18c85fc 614 IrRange[B_SPOT] = 0;
lilac0112_1 0:ea35c18c85fc 615 IrRange[C_SPOT] = 0;
lilac0112_1 0:ea35c18c85fc 616 IrRange[AB_SPOT] = 0;
lilac0112_1 0:ea35c18c85fc 617 }
lilac0112_1 0:ea35c18c85fc 618 //白線を踏み始めた方向とボールの方向が一致．(SelfHold)
lilac0112_1 0:ea35c18c85fc 619 LineBind[A_SPOT] = (IrRange[A_SPOT]==1)&&((LineDir[A_SPOT]==1)||(LineBind[A_SPOT]==1));
lilac0112_1 0:ea35c18c85fc 620 LineBind[B_SPOT] = (IrRange[B_SPOT]==1)&&((LineDir[B_SPOT]==1)||(LineBind[B_SPOT]==1));
lilac0112_1 0:ea35c18c85fc 621 LineBind[C_SPOT] = (IrRange[C_SPOT]==1)&&((LineDir[C_SPOT]==1)||(LineBind[C_SPOT]==1));
lilac0112_1 0:ea35c18c85fc 622 LineBind[AB_SPOT] = (IrRange[AB_SPOT]==1)&&((LineDir[AB_SPOT]==1)||(LineBind[AB_SPOT]==1));
lilac0112_1 0:ea35c18c85fc 623
lilac0112_1 0:ea35c18c85fc 624 LineStop[X_AXIS] = 1;//(LineBind[A_SPOT]==0)*(LineBind[B_SPOT]==0);
lilac0112_1 0:ea35c18c85fc 625 LineStop[Y_AXIS] = 1;//(LineBind[C_SPOT]==0)*(LineBind[AB_SPOT]==0);
lilac0112_1 0:ea35c18c85fc 626
lilac0112_1 0:ea35c18c85fc 627 //白線踏んでる
lilac0112_1 2:635947de1583 628 if(LineRaw>0){
lilac0112_1 0:ea35c18c85fc 629 LineOn[A_SPOT] = (LineSign[A_SPOT]==1) &&(LineFirst[X_AXIS]==A_SPOT);
lilac0112_1 0:ea35c18c85fc 630 LineOn[B_SPOT] = (LineSign[B_SPOT]==1) &&(LineFirst[X_AXIS]==B_SPOT);
lilac0112_1 0:ea35c18c85fc 631 LineOn[C_SPOT] = (LineSign[C_SPOT]==1) &&(LineFirst[Y_AXIS]==C_SPOT);
lilac0112_1 0:ea35c18c85fc 632 LineOn[AB_SPOT] = ((LineSign[A_SPOT]==1)&&(LineSign[B_SPOT]==1))&&(LineFirst[Y_AXIS]==AB_SPOT);
lilac0112_1 0:ea35c18c85fc 633 //外側に向かう力を消す．
lilac0112_1 0:ea35c18c85fc 634 //x
lilac0112_1 0:ea35c18c85fc 635 if(((LineOn[A_SPOT]==1)&&(vx>0))||((LineOn[B_SPOT]==1)&&(vx<0))){
lilac0112_1 0:ea35c18c85fc 636
lilac0112_1 0:ea35c18c85fc 637 if(LinePriority[Y_AXIS]==0){
lilac0112_1 0:ea35c18c85fc 638 LinePriority[X_AXIS]=1;
lilac0112_1 0:ea35c18c85fc 639 LinePriority[Y_AXIS]=0;
lilac0112_1 0:ea35c18c85fc 640 }
lilac0112_1 0:ea35c18c85fc 641 vx=0;
lilac0112_1 0:ea35c18c85fc 642 //yの力を加える．
lilac0112_1 0:ea35c18c85fc 643 if(vy>0){vy += 10;}
lilac0112_1 0:ea35c18c85fc 644 if(vy<0){vy -= 10;}
lilac0112_1 0:ea35c18c85fc 645 }
lilac0112_1 0:ea35c18c85fc 646 //y
lilac0112_1 0:ea35c18c85fc 647 if(((LineOn[C_SPOT]==1)&&(vy<0))||((LineOn[AB_SPOT]==1)&&(vy>0))){
lilac0112_1 0:ea35c18c85fc 648 if(LinePriority[X_AXIS]==0){
lilac0112_1 0:ea35c18c85fc 649 LinePriority[X_AXIS]=0;
lilac0112_1 0:ea35c18c85fc 650 LinePriority[Y_AXIS]=1;
lilac0112_1 0:ea35c18c85fc 651 }
lilac0112_1 0:ea35c18c85fc 652 if((LinePriority[X_AXIS]==1)&&(LineOn[AB_SPOT]==1)){
lilac0112_1 0:ea35c18c85fc 653 LinePriority[X_AXIS]=0;
lilac0112_1 0:ea35c18c85fc 654 LinePriority[Y_AXIS]=1;
lilac0112_1 0:ea35c18c85fc 655 }
lilac0112_1 0:ea35c18c85fc 656 vy=0;
lilac0112_1 0:ea35c18c85fc 657 }
lilac0112_1 0:ea35c18c85fc 658 //内側に向かう力を加える．
lilac0112_1 0:ea35c18c85fc 659 LineReturn[A_SPOT] = (LineOn[A_SPOT]==1);
lilac0112_1 0:ea35c18c85fc 660 /*if((LineReturn[A_SPOT]==1)&&(LineOn[AB_SPOT]==1)){
lilac0112_1 0:ea35c18c85fc 661 if(LineOn[C_SPOT]==0){
lilac0112_1 0:ea35c18c85fc 662 LineReturn[A_SPOT]=0;
lilac0112_1 0:ea35c18c85fc 663 }
lilac0112_1 0:ea35c18c85fc 664 else{
lilac0112_1 0:ea35c18c85fc 665 LineReturn[A_SPOT]=1;
lilac0112_1 0:ea35c18c85fc 666 }
lilac0112_1 0:ea35c18c85fc 667 }*/
lilac0112_1 0:ea35c18c85fc 668
lilac0112_1 0:ea35c18c85fc 669 LineReturn[B_SPOT] = (LineOn[B_SPOT]==1);
lilac0112_1 0:ea35c18c85fc 670 /*if((LineReturn[B_SPOT]==1)&&(LineOn[AB_SPOT]==1)){
lilac0112_1 0:ea35c18c85fc 671 if(LineOn[C_SPOT]==0){
lilac0112_1 0:ea35c18c85fc 672 LineReturn[B_SPOT]=0;
lilac0112_1 0:ea35c18c85fc 673 }
lilac0112_1 0:ea35c18c85fc 674 else{
lilac0112_1 0:ea35c18c85fc 675 LineReturn[B_SPOT]=1;
lilac0112_1 0:ea35c18c85fc 676 }
lilac0112_1 0:ea35c18c85fc 677 }*/
lilac0112_1 0:ea35c18c85fc 678 LineReturn[C_SPOT] = (LineOn[C_SPOT]==1);
lilac0112_1 0:ea35c18c85fc 679 /*if(LineReturn[C_SPOT]==1){
lilac0112_1 0:ea35c18c85fc 680 LineReturn[A_SPOT]=0;
lilac0112_1 0:ea35c18c85fc 681 LineReturn[B_SPOT]=0;
lilac0112_1 0:ea35c18c85fc 682 }*/
lilac0112_1 0:ea35c18c85fc 683 LineReturn[AB_SPOT] = (LineOn[AB_SPOT]==1);
lilac0112_1 0:ea35c18c85fc 684
lilac0112_1 0:ea35c18c85fc 685 LineForce[X_AXIS] = (LINE_RF*2)*(LinePriority[X_AXIS])*((-1)*(LineReturn[A_SPOT]==1) + ( 1)*(LineReturn[B_SPOT]==1)) +
lilac0112_1 2:635947de1583 686 (LINE_RF*2)*(LineReturn[AB_SPOT]==1)*(( 1)*(data.ping[L_PING]<40) + (-1)*(data.ping[R_PING]<40)) +
lilac0112_1 2:635947de1583 687 (LINE_RF*2)*(LineReturn[C_SPOT]==1)*(( 1)*(data.ping[L_PING]<40) + (-1)*(data.ping[R_PING]<40));
lilac0112_1 0:ea35c18c85fc 688 LineForce[Y_AXIS] = (LINE_RF*2)*(LinePriority[Y_AXIS])*(( 1)*(LineReturn[C_SPOT]==1) + (-1)*(LineReturn[AB_SPOT]==1));
lilac0112_1 0:ea35c18c85fc 689 /*
lilac0112_1 0:ea35c18c85fc 690 LineForce[X_AXIS] = (LINE_RF*2)*((-1)*(LineReturn[A_SPOT]==1) + ( 1)*(LineReturn[B_SPOT]==1)) +
lilac0112_1 0:ea35c18c85fc 691 (LINE_RF*2)*(LineReturn[AB_SPOT]==1)*(LineReturn[A_SPOT]==0)*(LineReturn[B_SPOT]==0)
lilac0112_1 2:635947de1583 692 *(( 1)*(data.ping[L_PING]<30) + (-1)*(data.ping[R_PING]<30)) +
lilac0112_1 0:ea35c18c85fc 693 (LINE_RF*2)*(LineReturn[C_SPOT]==1)*(LineReturn[A_SPOT]==0)*(LineReturn[B_SPOT]==0)
lilac0112_1 2:635947de1583 694 *(( 1)*(data.ping[L_PING]<30) + (-1)*(data.ping[R_PING]<30));
lilac0112_1 0:ea35c18c85fc 695 LineForce[Y_AXIS] = (LINE_RF*2)*(( 1)*(LineReturn[C_SPOT]==1) + (-1)*(LineReturn[AB_SPOT]==1));
lilac0112_1 0:ea35c18c85fc 696 */
lilac0112_1 0:ea35c18c85fc 697 Line_timeout[A_SPOT].attach(&LineClear_A, LINE_DELAY);
lilac0112_1 0:ea35c18c85fc 698 Line_timeout[B_SPOT].attach(&LineClear_B, LINE_DELAY);
lilac0112_1 0:ea35c18c85fc 699 Line_timeout[C_SPOT].attach(&LineClear_C, LINE_DELAY);
lilac0112_1 0:ea35c18c85fc 700 }
lilac0112_1 0:ea35c18c85fc 701 else{
lilac0112_1 0:ea35c18c85fc 702 LineForce[X_AXIS] = 0;
lilac0112_1 0:ea35c18c85fc 703 LineForce[Y_AXIS] = 0;
lilac0112_1 0:ea35c18c85fc 704
lilac0112_1 0:ea35c18c85fc 705 LinePriority[X_AXIS]=0;
lilac0112_1 0:ea35c18c85fc 706 LinePriority[Y_AXIS]=0;
lilac0112_1 0:ea35c18c85fc 707 }
lilac0112_1 0:ea35c18c85fc 708
lilac0112_1 0:ea35c18c85fc 709 vx = vx*LineStop[X_AXIS] + LineForce[X_AXIS];
lilac0112_1 0:ea35c18c85fc 710 vy = vy*LineStop[Y_AXIS] + LineForce[Y_AXIS];
lilac0112_1 0:ea35c18c85fc 711 vs = cmps_set.OutputPID;
lilac0112_1 0:ea35c18c85fc 712 move(
lilac0112_1 0:ea35c18c85fc 713 vx,
lilac0112_1 0:ea35c18c85fc 714 vy,
lilac0112_1 0:ea35c18c85fc 715 vs
lilac0112_1 0:ea35c18c85fc 716 );
lilac0112_1 0:ea35c18c85fc 717 /*move(
lilac0112_1 0:ea35c18c85fc 718 10,
lilac0112_1 0:ea35c18c85fc 719 0,
lilac0112_1 0:ea35c18c85fc 720 0
lilac0112_1 0:ea35c18c85fc 721 );*/
lilac0112_1 0:ea35c18c85fc 722 if(sys.MotorFlag==1){
lilac0112_1 0:ea35c18c85fc 723 tx_motor();
lilac0112_1 0:ea35c18c85fc 724 sys.MotorFlag=0;
lilac0112_1 0:ea35c18c85fc 725 }
lilac0112_1 0:ea35c18c85fc 726 if(sys.stopflag==1){
lilac0112_1 0:ea35c18c85fc 727 //停止処理
lilac0112_1 0:ea35c18c85fc 728 }
lilac0112_1 0:ea35c18c85fc 729 return;
lilac0112_1 0:ea35c18c85fc 730 }
lilac0112_1 14:b510adcb6065 731 void LineJudgeSlow(double pow_x, double pow_y, double *x, double *y){
lilac0112_1 11:3efae754e6ef 732 uint8_t LineState;
lilac0112_1 11:3efae754e6ef 733
lilac0112_1 11:3efae754e6ef 734 //line
lilac0112_1 11:3efae754e6ef 735 LineState = 0;
lilac0112_1 11:3efae754e6ef 736 if(data.lnHold==7){
lilac0112_1 11:3efae754e6ef 737 if(data.lnRaw>0){
lilac0112_1 11:3efae754e6ef 738 //場外間際...減衰
lilac0112_1 11:3efae754e6ef 739 LineState = 3;
lilac0112_1 11:3efae754e6ef 740 }
lilac0112_1 11:3efae754e6ef 741 else{//data.lnRaw==0
lilac0112_1 11:3efae754e6ef 742 //場外...出力完全無効+反発
lilac0112_1 11:3efae754e6ef 743 //powerVoid & restore
lilac0112_1 11:3efae754e6ef 744 LineState = 4;
lilac0112_1 11:3efae754e6ef 745 }
lilac0112_1 11:3efae754e6ef 746 }
lilac0112_1 11:3efae754e6ef 747 else if(data.lnHold>0){
lilac0112_1 11:3efae754e6ef 748 if(data.lnRaw>0){
lilac0112_1 11:3efae754e6ef 749 //踏んでるけどまだ出てない...減衰
lilac0112_1 11:3efae754e6ef 750 LineState = 2;
lilac0112_1 11:3efae754e6ef 751 //timeout&reset...detach
lilac0112_1 11:3efae754e6ef 752 }
lilac0112_1 11:3efae754e6ef 753 else{//data.lnRaw==0
lilac0112_1 11:3efae754e6ef 754 //線をまたいでいるか，中途半端に線を踏んだあと復帰したか...減衰
lilac0112_1 11:3efae754e6ef 755 LineState = 1;
lilac0112_1 11:3efae754e6ef 756 //ping&reset
lilac0112_1 11:3efae754e6ef 757 //timeout&reset
lilac0112_1 11:3efae754e6ef 758 }
lilac0112_1 11:3efae754e6ef 759 }
lilac0112_1 11:3efae754e6ef 760 //else if(data.lnHold==0){...maxpower
lilac0112_1 13:b20921316f3c 761 if(data.FieldState==LINE_OUTSIDE){
lilac0112_1 13:b20921316f3c 762 LineState = 4;
lilac0112_1 13:b20921316f3c 763 }
lilac0112_1 13:b20921316f3c 764
lilac0112_1 11:3efae754e6ef 765
lilac0112_1 14:b510adcb6065 766 if(pow_x>=0){
lilac0112_1 11:3efae754e6ef 767 if(data.ping[R_PING]<WhiteToWall[X_PING]) (*x)=LineDecline[LineState];
lilac0112_1 11:3efae754e6ef 768 else *x=1;
lilac0112_1 11:3efae754e6ef 769 }
lilac0112_1 11:3efae754e6ef 770 else{
lilac0112_1 11:3efae754e6ef 771 if(data.ping[L_PING]<WhiteToWall[X_PING]) (*x)=LineDecline[LineState];
lilac0112_1 11:3efae754e6ef 772 else *x=1;
lilac0112_1 11:3efae754e6ef 773 }
lilac0112_1 11:3efae754e6ef 774
lilac0112_1 14:b510adcb6065 775 if(pow_y>=0){
lilac0112_1 11:3efae754e6ef 776 if(data.ping[F_PING]<WhiteToWall[Y_PING]) (*y)=LineDecline[LineState];
lilac0112_1 11:3efae754e6ef 777 else *y=1;
lilac0112_1 11:3efae754e6ef 778 }
lilac0112_1 11:3efae754e6ef 779 else{
lilac0112_1 11:3efae754e6ef 780 if(data.ping[B_PING]<WhiteToWall[Y_PING]) (*y)=LineDecline[LineState];
lilac0112_1 11:3efae754e6ef 781 else *y=1;
lilac0112_1 11:3efae754e6ef 782 }
lilac0112_1 11:3efae754e6ef 783 }
lilac0112_1 13:b20921316f3c 784 void LineJudgeReturn(double pow_x, double pow_y, double *x, double *y){
lilac0112_1 12:bee8f883c33a 785 const int8_t static LineReturn[5] = {0, 0, 0, 0, 10};
lilac0112_1 12:bee8f883c33a 786 uint8_t LinePingState[4];
lilac0112_1 12:bee8f883c33a 787 //◎ボールを追う力とラインから離れる力の向きが違うならばラインから離れる力が優先される．
lilac0112_1 12:bee8f883c33a 788 //◎ボールを追う力とラインから離れる力の向きが同じならばボールを追う力が優先される．
lilac0112_1 12:bee8f883c33a 789 //◎ラインセンサ全てが場外になるまではボールを追う力は作用しない．ボールを追う力は場内に出るまで作用する．
lilac0112_1 12:bee8f883c33a 790 //×ラインのほぼ場外では常時ラインから離れる力が優先される．
lilac0112_1 12:bee8f883c33a 791 //※場外判定を行うには再び場内に戻る必要がある．
lilac0112_1 12:bee8f883c33a 792
lilac0112_1 12:bee8f883c33a 793 if(data.FieldState==LINE_OUTSIDE){
lilac0112_1 12:bee8f883c33a 794
lilac0112_1 12:bee8f883c33a 795 LinePingState[L_PING]=(data.ping[L_PING]<WhiteToWall[X_PING]);
lilac0112_1 12:bee8f883c33a 796 LinePingState[R_PING]=(data.ping[R_PING]<WhiteToWall[X_PING]);
lilac0112_1 12:bee8f883c33a 797 LinePingState[F_PING]=(data.ping[F_PING]<WhiteToWall[Y_PING]);
lilac0112_1 12:bee8f883c33a 798 LinePingState[B_PING]=(data.ping[B_PING]<WhiteToWall[Y_PING]);
lilac0112_1 12:bee8f883c33a 799
lilac0112_1 12:bee8f883c33a 800 //line間際の復帰力以外の力を作用させるか否か
lilac0112_1 12:bee8f883c33a 801 data.lnStop[X_LINE]=1;
lilac0112_1 12:bee8f883c33a 802 data.lnStop[Y_LINE]=1;
lilac0112_1 12:bee8f883c33a 803 //x
lilac0112_1 12:bee8f883c33a 804 if((LinePingState[L_PING]==0)&&(LinePingState[R_PING]==0)){
lilac0112_1 13:b20921316f3c 805 if(pow_x>0){
lilac0112_1 13:b20921316f3c 806 *x = -pow_x*(1-0.75);
lilac0112_1 13:b20921316f3c 807 }
lilac0112_1 13:b20921316f3c 808 else{
lilac0112_1 13:b20921316f3c 809 *x = pow_x*(1-0.75);
lilac0112_1 13:b20921316f3c 810 }
lilac0112_1 13:b20921316f3c 811 data.lnStop[X_LINE]=1;
lilac0112_1 12:bee8f883c33a 812 }
lilac0112_1 12:bee8f883c33a 813 if((LinePingState[L_PING]==0)&&(LinePingState[R_PING]==1)){
lilac0112_1 13:b20921316f3c 814 if(pow_x<0){
lilac0112_1 13:b20921316f3c 815 *x = 0;
lilac0112_1 13:b20921316f3c 816 data.lnStop[X_LINE]=1;
lilac0112_1 13:b20921316f3c 817 }
lilac0112_1 13:b20921316f3c 818 else{
lilac0112_1 13:b20921316f3c 819 *x = -LineReturn[4];
lilac0112_1 13:b20921316f3c 820 data.lnStop[X_LINE]=0;
lilac0112_1 13:b20921316f3c 821 }
lilac0112_1 12:bee8f883c33a 822 }
lilac0112_1 12:bee8f883c33a 823 if((LinePingState[L_PING]==1)&&(LinePingState[R_PING]==0)){
lilac0112_1 13:b20921316f3c 824 if(pow_x>0){
lilac0112_1 13:b20921316f3c 825 *x = 0;
lilac0112_1 13:b20921316f3c 826 data.lnStop[X_LINE]=1;
lilac0112_1 13:b20921316f3c 827 }
lilac0112_1 13:b20921316f3c 828 else{
lilac0112_1 13:b20921316f3c 829 *x = LineReturn[4];
lilac0112_1 13:b20921316f3c 830 data.lnStop[X_LINE]=0;
lilac0112_1 13:b20921316f3c 831 }
lilac0112_1 12:bee8f883c33a 832 }
lilac0112_1 12:bee8f883c33a 833 if((LinePingState[L_PING]==1)&&(LinePingState[R_PING]==1)){
lilac0112_1 12:bee8f883c33a 834 *x = 0;
lilac0112_1 13:b20921316f3c 835 data.lnStop[X_LINE]=0;
lilac0112_1 12:bee8f883c33a 836 }
lilac0112_1 12:bee8f883c33a 837
lilac0112_1 12:bee8f883c33a 838 //y
lilac0112_1 13:b20921316f3c 839 if((LinePingState[B_PING]==0)&&(LinePingState[F_PING]==0)){
lilac0112_1 13:b20921316f3c 840 if(pow_y>0){
lilac0112_1 13:b20921316f3c 841 *y = -pow_y*(1-0.75);
lilac0112_1 13:b20921316f3c 842 }
lilac0112_1 13:b20921316f3c 843 else{
lilac0112_1 13:b20921316f3c 844 *y = pow_y*(1-0.75);
lilac0112_1 13:b20921316f3c 845 }
lilac0112_1 13:b20921316f3c 846 data.lnStop[Y_LINE]=1;
lilac0112_1 12:bee8f883c33a 847 }
lilac0112_1 13:b20921316f3c 848 if((LinePingState[B_PING]==0)&&(LinePingState[F_PING]==1)){
lilac0112_1 13:b20921316f3c 849 if(pow_y<0){
lilac0112_1 13:b20921316f3c 850 *y = 0;
lilac0112_1 13:b20921316f3c 851 data.lnStop[Y_LINE]=1;
lilac0112_1 13:b20921316f3c 852 }
lilac0112_1 13:b20921316f3c 853 else{
lilac0112_1 13:b20921316f3c 854 *y = -LineReturn[4];
lilac0112_1 13:b20921316f3c 855 data.lnStop[Y_LINE]=0;
lilac0112_1 13:b20921316f3c 856 }
lilac0112_1 12:bee8f883c33a 857 }
lilac0112_1 13:b20921316f3c 858 if((LinePingState[B_PING]==1)&&(LinePingState[F_PING]==0)){
lilac0112_1 13:b20921316f3c 859 if(pow_y>0){
lilac0112_1 13:b20921316f3c 860 *y = 0;
lilac0112_1 13:b20921316f3c 861 data.lnStop[Y_LINE]=1;
lilac0112_1 13:b20921316f3c 862 }
lilac0112_1 13:b20921316f3c 863 else{
lilac0112_1 13:b20921316f3c 864 *y = LineReturn[4];
lilac0112_1 13:b20921316f3c 865 data.lnStop[Y_LINE]=0;
lilac0112_1 13:b20921316f3c 866 }
lilac0112_1 12:bee8f883c33a 867 }
lilac0112_1 13:b20921316f3c 868 if((LinePingState[B_PING]==1)&&(LinePingState[F_PING]==1)){
lilac0112_1 13:b20921316f3c 869 *y = 0;
lilac0112_1 13:b20921316f3c 870 data.lnStop[Y_LINE]=0;
lilac0112_1 12:bee8f883c33a 871 }
lilac0112_1 13:b20921316f3c 872
lilac0112_1 13:b20921316f3c 873
lilac0112_1 12:bee8f883c33a 874 }
lilac0112_1 12:bee8f883c33a 875 else{//data.FieldState==LINE_INSIDE
lilac0112_1 12:bee8f883c33a 876 *x = 0;
lilac0112_1 12:bee8f883c33a 877 *y = 0;
lilac0112_1 12:bee8f883c33a 878
lilac0112_1 12:bee8f883c33a 879 //line間際の復帰力以外の力を作用させるか否か
lilac0112_1 12:bee8f883c33a 880 data.lnStop[X_LINE]=1;
lilac0112_1 12:bee8f883c33a 881 data.lnStop[Y_LINE]=1;
lilac0112_1 12:bee8f883c33a 882 }
lilac0112_1 12:bee8f883c33a 883 }
lilac0112_1 12:bee8f883c33a 884 void LineJudgeReset(void){
lilac0112_1 14:b510adcb6065 885 //uint8_t LineCorner[4];
lilac0112_1 13:b20921316f3c 886
lilac0112_1 14:b510adcb6065 887 if((data.lnRaw==0)&&(data.lnHold==7)){
lilac0112_1 14:b510adcb6065 888 if(data.FieldState==LINE_INSIDE){
lilac0112_1 14:b510adcb6065 889 data.lnCorner[L_LINE] = (data.ping[L_PING]<OutToWall[X_PING]);
lilac0112_1 14:b510adcb6065 890 data.lnCorner[R_LINE] = (data.ping[R_PING]<OutToWall[X_PING]);
lilac0112_1 14:b510adcb6065 891 data.lnCorner[F_LINE] = (data.ping[F_PING]<OutToWall[Y_PING]);
lilac0112_1 14:b510adcb6065 892 data.lnCorner[B_LINE] = (data.ping[B_PING]<OutToWall[Y_PING]);
lilac0112_1 14:b510adcb6065 893 if(
lilac0112_1 14:b510adcb6065 894 (data.lnCorner[L_LINE])||
lilac0112_1 14:b510adcb6065 895 (data.lnCorner[R_LINE])||
lilac0112_1 14:b510adcb6065 896 (data.lnCorner[F_LINE])||
lilac0112_1 14:b510adcb6065 897 (data.lnCorner[B_LINE])
lilac0112_1 14:b510adcb6065 898 ){
lilac0112_1 14:b510adcb6065 899 LineLiberate();
lilac0112_1 14:b510adcb6065 900 data.FieldState=LINE_OUTSIDE;
lilac0112_1 14:b510adcb6065 901 }
lilac0112_1 14:b510adcb6065 902 }
lilac0112_1 14:b510adcb6065 903 else{//data.FieldState==LINE_OUTSIDE
lilac0112_1 14:b510adcb6065 904 if(
lilac0112_1 14:b510adcb6065 905 (data.ping[L_PING]>=WhiteToWall[X_PING])&&
lilac0112_1 14:b510adcb6065 906 (data.ping[R_PING]>=WhiteToWall[X_PING])
lilac0112_1 14:b510adcb6065 907 ){
lilac0112_1 14:b510adcb6065 908 LineLiberate();
lilac0112_1 14:b510adcb6065 909 data.FieldState=LINE_OUTSIDE;
lilac0112_1 14:b510adcb6065 910 }
lilac0112_1 14:b510adcb6065 911 }
lilac0112_1 14:b510adcb6065 912 }
lilac0112_1 14:b510adcb6065 913 /*
lilac0112_1 13:b20921316f3c 914 //line
lilac0112_1 13:b20921316f3c 915 if(data.lnHold==7){
lilac0112_1 13:b20921316f3c 916 if(data.lnRaw>0){
lilac0112_1 13:b20921316f3c 917 //場外間際...減衰
lilac0112_1 13:b20921316f3c 918 }
lilac0112_1 13:b20921316f3c 919 else{//data.lnRaw==0
lilac0112_1 13:b20921316f3c 920 //場外...出力完全無効+反発
lilac0112_1 14:b510adcb6065 921
lilac0112_1 14:b510adcb6065 922 data.lnCorner[L_LINE] = (data.ping[L_PING]<OutToWall[X_PING]);
lilac0112_1 14:b510adcb6065 923 data.lnCorner[R_LINE] = (data.ping[R_PING]<OutToWall[X_PING]);
lilac0112_1 14:b510adcb6065 924 data.lnCorner[F_LINE] = (data.ping[F_PING]<OutToWall[Y_PING]);
lilac0112_1 14:b510adcb6065 925 data.lnCorner[B_LINE] = (data.ping[B_PING]<OutToWall[Y_PING]);
lilac0112_1 13:b20921316f3c 926 if(
lilac0112_1 14:b510adcb6065 927 (data.lnCorner[L_LINE])||
lilac0112_1 14:b510adcb6065 928 (data.lnCorner[R_LINE])||
lilac0112_1 14:b510adcb6065 929 (data.lnCorner[F_LINE])||
lilac0112_1 14:b510adcb6065 930 (data.lnCorner[B_LINE])
lilac0112_1 13:b20921316f3c 931 ){
lilac0112_1 14:b510adcb6065 932 //LineLiberate();
lilac0112_1 13:b20921316f3c 933 data.FieldState=LINE_OUTSIDE;
lilac0112_1 13:b20921316f3c 934 }
lilac0112_1 13:b20921316f3c 935 if(
lilac0112_1 13:b20921316f3c 936 (data.ping[L_PING]>=WhiteToWall[X_PING])&&
lilac0112_1 13:b20921316f3c 937 (data.ping[R_PING]>=WhiteToWall[X_PING])
lilac0112_1 13:b20921316f3c 938 ){
lilac0112_1 14:b510adcb6065 939 //LineLiberate();
lilac0112_1 14:b510adcb6065 940 //data.FieldState=LINE_INSIDE;
lilac0112_1 13:b20921316f3c 941 }
lilac0112_1 14:b510adcb6065 942 //LineLiberate();
lilac0112_1 14:b510adcb6065 943 //data.FieldState=!data.FieldState;
lilac0112_1 13:b20921316f3c 944 }
lilac0112_1 13:b20921316f3c 945 }
lilac0112_1 13:b20921316f3c 946 else if(data.lnHold>0){
lilac0112_1 13:b20921316f3c 947 if(data.lnRaw>0){
lilac0112_1 13:b20921316f3c 948 //踏んでるけどまだ出てない...減衰
lilac0112_1 13:b20921316f3c 949 }
lilac0112_1 13:b20921316f3c 950 else{//data.lnRaw==0
lilac0112_1 13:b20921316f3c 951 //線をまたいでいるか，中途半端に線を踏んだあと復帰したか...減衰
lilac0112_1 14:b510adcb6065 952 //if((data.ping[L_PING]>WhiteToWall[X_PING])&&(data.ping[R_PING]>WhiteToWall[X_PING])) LineLiberate();
lilac0112_1 13:b20921316f3c 953 }
lilac0112_1 13:b20921316f3c 954 }
lilac0112_1 13:b20921316f3c 955 //else if(data.lnHold==0){...maxpower
lilac0112_1 14:b510adcb6065 956 */
lilac0112_1 12:bee8f883c33a 957 }
lilac0112_1 0:ea35c18c85fc 958 void modeAttack4(void){
lilac0112_1 10:6df631c39f9b 959 double ir_x_dir, ir_y_dir;
lilac0112_1 11:3efae754e6ef 960 double ir_x_turn, ir_y_turn;
lilac0112_1 11:3efae754e6ef 961 double ir_x, ir_y;
lilac0112_1 13:b20921316f3c 962
lilac0112_1 14:b510adcb6065 963 double LineSlowPower[2];
lilac0112_1 14:b510adcb6065 964 double LineReturnPower[2];
lilac0112_1 14:b510adcb6065 965
lilac0112_1 14:b510adcb6065 966 //double pow_x, pow_y;
lilac0112_1 10:6df631c39f9b 967 uint8_t ir_pow;
lilac0112_1 10:6df631c39f9b 968 int vx,vy,vs;
lilac0112_1 15:17502a27a60b 969 //static uint8_t data.FieldSpot;
lilac0112_1 11:3efae754e6ef 970
lilac0112_1 11:3efae754e6ef 971
lilac0112_1 10:6df631c39f9b 972 if(sys.KickOffFlag==1){
lilac0112_1 10:6df631c39f9b 973
lilac0112_1 14:b510adcb6065 974 sys.IrBlind=1;
lilac0112_1 10:6df631c39f9b 975 sys.LineBlind=0;
lilac0112_1 10:6df631c39f9b 976 sys.PingBlind=0;
lilac0112_1 10:6df631c39f9b 977
lilac0112_1 15:17502a27a60b 978 data.FieldSpot = LINE_INSIDE;
lilac0112_1 14:b510adcb6065 979 LineLiberate();
lilac0112_1 13:b20921316f3c 980
lilac0112_1 10:6df631c39f9b 981 sys.KickOffFlag=0;
lilac0112_1 0:ea35c18c85fc 982 }
lilac0112_1 10:6df631c39f9b 983 //data
lilac0112_1 10:6df631c39f9b 984 if(sys.IrFlag==1){ReadIr();sys.IrFlag=0;}
lilac0112_1 10:6df631c39f9b 985 if(sys.PidFlag==1){PidUpdate();sys.PidFlag=0;}
lilac0112_1 10:6df631c39f9b 986 if(sys.UswFlag==1){ReadPing();sys.UswFlag=0;}
lilac0112_1 10:6df631c39f9b 987 if(sys.UswFlag2==1){ReadPing2();sys.UswFlag2=0;}
lilac0112_1 10:6df631c39f9b 988 //if(sys.KickFlag==1){DriveSolenoid();}
lilac0112_1 11:3efae754e6ef 989 data.lnRaw = LineRaw;
lilac0112_1 11:3efae754e6ef 990 data.lnHold = LineHold;
lilac0112_1 10:6df631c39f9b 991
lilac0112_1 10:6df631c39f9b 992 ir_x_dir = ir_move_val[0][data.irNotice][data.irPosition][IR_X_DIR];
lilac0112_1 10:6df631c39f9b 993 ir_y_dir = ir_move_val[0][data.irNotice][data.irPosition][IR_Y_DIR];
lilac0112_1 10:6df631c39f9b 994 ir_x_turn = ir_move_val[0][data.irNotice][data.irPosition][IR_X_TURN];
lilac0112_1 10:6df631c39f9b 995 ir_y_turn = ir_move_val[0][data.irNotice][data.irPosition][IR_Y_TURN];
lilac0112_1 10:6df631c39f9b 996 if(data.irNotice==IR_CLOSER){
lilac0112_1 10:6df631c39f9b 997 ir_pow = sys.s_pow;
lilac0112_1 10:6df631c39f9b 998 }
lilac0112_1 10:6df631c39f9b 999 else if(data.irNotice==IR_CLOSE){
lilac0112_1 10:6df631c39f9b 1000 ir_pow = sys.m_pow;
lilac0112_1 10:6df631c39f9b 1001 }
lilac0112_1 10:6df631c39f9b 1002 else if(data.irNotice==IR_FAR){
lilac0112_1 10:6df631c39f9b 1003 ir_pow = sys.l_pow;
lilac0112_1 10:6df631c39f9b 1004 }
lilac0112_1 10:6df631c39f9b 1005 else{//data.irNotice==IR_NONE
lilac0112_1 10:6df631c39f9b 1006 ir_pow = 0;
lilac0112_1 10:6df631c39f9b 1007 }
lilac0112_1 10:6df631c39f9b 1008 if(sys.IrBlind==1) data.irNotice=IR_NONE;
lilac0112_1 10:6df631c39f9b 1009
lilac0112_1 11:3efae754e6ef 1010 ir_x = (ir_x_dir + ir_x_turn);
lilac0112_1 11:3efae754e6ef 1011 ir_y = (ir_y_dir + ir_y_turn);
lilac0112_1 11:3efae754e6ef 1012
lilac0112_1 14:b510adcb6065 1013 if(sys.LineBlind==1){
lilac0112_1 14:b510adcb6065 1014
lilac0112_1 14:b510adcb6065 1015 LineSlowPower[X_LINE] = 1.0;
lilac0112_1 14:b510adcb6065 1016 LineSlowPower[Y_LINE] = 1.0;
lilac0112_1 14:b510adcb6065 1017
lilac0112_1 14:b510adcb6065 1018 LineReturnPower[X_LINE] = 0.0;
lilac0112_1 14:b510adcb6065 1019 LineReturnPower[Y_LINE] = 0.0;
lilac0112_1 14:b510adcb6065 1020
lilac0112_1 14:b510adcb6065 1021 data.lnStop[X_LINE] = 1;
lilac0112_1 14:b510adcb6065 1022 data.lnStop[Y_LINE] = 1;
lilac0112_1 14:b510adcb6065 1023
lilac0112_1 15:17502a27a60b 1024 data.FieldSpot = LINE_INSIDE;
lilac0112_1 14:b510adcb6065 1025 }
lilac0112_1 14:b510adcb6065 1026 else{
lilac0112_1 14:b510adcb6065 1027 //LineJudgeReset();
lilac0112_1 14:b510adcb6065 1028 if((data.lnRaw==0)&&(data.lnHold==7)){
lilac0112_1 15:17502a27a60b 1029 if(data.FieldSpot==LINE_INSIDE){
lilac0112_1 14:b510adcb6065 1030 data.lnCorner[L_LINE] = (data.ping[L_PING]<OutToWall[X_PING]);
lilac0112_1 14:b510adcb6065 1031 data.lnCorner[R_LINE] = (data.ping[R_PING]<OutToWall[X_PING]);
lilac0112_1 14:b510adcb6065 1032 data.lnCorner[F_LINE] = (data.ping[F_PING]<OutToWall[Y_PING]);
lilac0112_1 14:b510adcb6065 1033 data.lnCorner[B_LINE] = (data.ping[B_PING]<OutToWall[Y_PING]);
lilac0112_1 14:b510adcb6065 1034 if(
lilac0112_1 14:b510adcb6065 1035 (data.lnCorner[L_LINE])||
lilac0112_1 14:b510adcb6065 1036 (data.lnCorner[R_LINE])||
lilac0112_1 14:b510adcb6065 1037 (data.lnCorner[F_LINE])||
lilac0112_1 14:b510adcb6065 1038 (data.lnCorner[B_LINE])
lilac0112_1 14:b510adcb6065 1039 ){
lilac0112_1 15:17502a27a60b 1040 data.FieldSpot = LINE_OUTSIDE;
lilac0112_1 15:17502a27a60b 1041 LineLiberate();
lilac0112_1 14:b510adcb6065 1042 }
lilac0112_1 15:17502a27a60b 1043 }
lilac0112_1 15:17502a27a60b 1044 else if(data.FieldSpot==LINE_OUTSIDE){
lilac0112_1 15:17502a27a60b 1045 data.FieldSpot = LINE_INSIDE;
lilac0112_1 14:b510adcb6065 1046 LineLiberate();
lilac0112_1 14:b510adcb6065 1047 }
lilac0112_1 15:17502a27a60b 1048 }
lilac0112_1 15:17502a27a60b 1049 if(data.FieldSpot == LINE_OUTSIDE){
lilac0112_1 15:17502a27a60b 1050 if(
lilac0112_1 15:17502a27a60b 1051 (data.ping[L_PING]>=WhiteToWall[X_PING])&&
lilac0112_1 15:17502a27a60b 1052 (data.ping[R_PING]>=WhiteToWall[X_PING])&&
lilac0112_1 15:17502a27a60b 1053 (data.ping[F_PING]>=WhiteToWall[Y_PING])&&
lilac0112_1 15:17502a27a60b 1054 (data.ping[B_PING]>=WhiteToWall[Y_PING])
lilac0112_1 15:17502a27a60b 1055 ){
lilac0112_1 15:17502a27a60b 1056 data.FieldSpot = LINE_INSIDE;
lilac0112_1 15:17502a27a60b 1057 }
lilac0112_1 15:17502a27a60b 1058 }
lilac0112_1 15:17502a27a60b 1059 if((data.FieldSpot == LINE_INSIDE)&&(0<data.lnHold)&&(data.lnHold<7)&&(data.lnRaw==0)){
lilac0112_1 15:17502a27a60b 1060 if(
lilac0112_1 15:17502a27a60b 1061 (data.ping[L_PING]>=WhiteToWall[X_PING])&&
lilac0112_1 15:17502a27a60b 1062 (data.ping[R_PING]>=WhiteToWall[X_PING])&&
lilac0112_1 15:17502a27a60b 1063 (data.ping[F_PING]>=WhiteToWall[Y_PING])&&
lilac0112_1 15:17502a27a60b 1064 (data.ping[B_PING]>=WhiteToWall[Y_PING])
lilac0112_1 15:17502a27a60b 1065 ){
lilac0112_1 14:b510adcb6065 1066 LineLiberate();
lilac0112_1 14:b510adcb6065 1067 }
lilac0112_1 14:b510adcb6065 1068 }
lilac0112_1 14:b510adcb6065 1069 //LineJudgeSlow(ir_x, ir_y, &LineSlowPower[X_LINE], &LineSlowPower[Y_LINE]);
lilac0112_1 14:b510adcb6065 1070 //LineJudgeReturn(ir_x, ir_y, &LineReturnPower[X_LINE], &LineReturnPower[Y_LINE]);
lilac0112_1 14:b510adcb6065 1071
lilac0112_1 14:b510adcb6065 1072 LineSlowPower[X_LINE] = 1.0;
lilac0112_1 14:b510adcb6065 1073 LineSlowPower[Y_LINE] = 1.0;
lilac0112_1 14:b510adcb6065 1074
lilac0112_1 14:b510adcb6065 1075 LineReturnPower[X_LINE] = 0.0;
lilac0112_1 14:b510adcb6065 1076 LineReturnPower[Y_LINE] = 0.0;
lilac0112_1 14:b510adcb6065 1077
lilac0112_1 14:b510adcb6065 1078 data.lnStop[X_LINE] = 1;
lilac0112_1 14:b510adcb6065 1079 data.lnStop[Y_LINE] = 1;
lilac0112_1 14:b510adcb6065 1080
lilac0112_1 14:b510adcb6065 1081 }
lilac0112_1 15:17502a27a60b 1082 if(data.FieldSpot==LINE_OUTSIDE) LED = 0x9;
lilac0112_1 15:17502a27a60b 1083 if(data.FieldSpot==LINE_INSIDE) LED = 0x6;
lilac0112_1 14:b510adcb6065 1084
lilac0112_1 14:b510adcb6065 1085
lilac0112_1 14:b510adcb6065 1086 //else LED = 0xA;
lilac0112_1 14:b510adcb6065 1087 //LED = LineHold;
lilac0112_1 14:b510adcb6065 1088
lilac0112_1 14:b510adcb6065 1089 vx = (ir_pow*ir_x)*data.lnStop[X_LINE]*LineSlowPower[X_LINE] + LineReturnPower[X_LINE];
lilac0112_1 14:b510adcb6065 1090 vy = (ir_pow*ir_y)*data.lnStop[Y_LINE]*LineSlowPower[Y_LINE] + LineReturnPower[Y_LINE];
lilac0112_1 10:6df631c39f9b 1091 vs = cmps_set.OutputPID;
lilac0112_1 10:6df631c39f9b 1092 move(
lilac0112_1 10:6df631c39f9b 1093 vx,
lilac0112_1 10:6df631c39f9b 1094 vy,
lilac0112_1 10:6df631c39f9b 1095 vs
lilac0112_1 10:6df631c39f9b 1096 );
lilac0112_1 10:6df631c39f9b 1097 if(sys.MotorFlag==1){tx_motor();sys.MotorFlag=0;}
lilac0112_1 0:ea35c18c85fc 1098 if(sys.stopflag==1){
lilac0112_1 10:6df631c39f9b 1099
lilac0112_1 0:ea35c18c85fc 1100 //停止処理
lilac0112_1 0:ea35c18c85fc 1101 }
lilac0112_1 0:ea35c18c85fc 1102 return;
lilac0112_1 0:ea35c18c85fc 1103 }
lilac0112_1 0:ea35c18c85fc 1104 void modeAttack5(void){
lilac0112_1 0:ea35c18c85fc 1105 if(sys.IrFlag==1){
lilac0112_1 0:ea35c18c85fc 1106 ReadIr();
lilac0112_1 0:ea35c18c85fc 1107 sys.IrFlag=0;
lilac0112_1 0:ea35c18c85fc 1108 }
lilac0112_1 0:ea35c18c85fc 1109 if(sys.PidFlag==1){
lilac0112_1 0:ea35c18c85fc 1110 PidUpdate();
lilac0112_1 0:ea35c18c85fc 1111 sys.PidFlag=0;
lilac0112_1 0:ea35c18c85fc 1112 }
lilac0112_1 10:6df631c39f9b 1113 move(0,0,cmps_set.OutputPID);
lilac0112_1 0:ea35c18c85fc 1114 if(sys.MotorFlag==1){
lilac0112_1 14:b510adcb6065 1115 //LED[0] = 1;
lilac0112_1 14:b510adcb6065 1116 //LED[1] = 0;
lilac0112_1 0:ea35c18c85fc 1117 tx_motor();
lilac0112_1 0:ea35c18c85fc 1118 sys.MotorFlag=0;
lilac0112_1 0:ea35c18c85fc 1119 }
lilac0112_1 5:5ff3a7d5d8c2 1120 else{
lilac0112_1 14:b510adcb6065 1121 //LED[0] = 0;
lilac0112_1 14:b510adcb6065 1122 //LED[1] = 1;
lilac0112_1 5:5ff3a7d5d8c2 1123 }
lilac0112_1 0:ea35c18c85fc 1124 if(sys.stopflag==1){
lilac0112_1 0:ea35c18c85fc 1125 //停止処理
lilac0112_1 0:ea35c18c85fc 1126 }
lilac0112_1 0:ea35c18c85fc 1127 return;
lilac0112_1 0:ea35c18c85fc 1128 }