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@16:4fadb7a87497, 2016-03-14 (annotated)

Committer:
lilac0112_1
Date:
Mon Mar 14 07:51:37 2016 +0000
Revision:
16:4fadb7a87497
Parent:
15:17502a27a60b
Child:
17:cc862ecf9812
line line line

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 16:4fadb7a87497 733 uint8_t LineSum;
lilac0112_1 11:3efae754e6ef 734
lilac0112_1 11:3efae754e6ef 735 //line
lilac0112_1 11:3efae754e6ef 736 LineState = 0;
lilac0112_1 16:4fadb7a87497 737
lilac0112_1 16:4fadb7a87497 738 LineSum = ((data.lnHold>>2)&0x1)+((data.lnHold>>1)&0x1)+((data.lnHold>>0)&0x1);
lilac0112_1 16:4fadb7a87497 739 if((LineSum==3)||(data.FieldSpot==LINE_OUTSIDE)){
lilac0112_1 16:4fadb7a87497 740 LineState = 3;
lilac0112_1 16:4fadb7a87497 741 }
lilac0112_1 16:4fadb7a87497 742 else if(LineSum==2){
lilac0112_1 16:4fadb7a87497 743 LineState = 2;
lilac0112_1 16:4fadb7a87497 744 }
lilac0112_1 16:4fadb7a87497 745 else if(LineSum==1){
lilac0112_1 16:4fadb7a87497 746 LineState = 1;
lilac0112_1 16:4fadb7a87497 747 }
lilac0112_1 16:4fadb7a87497 748 else if(LineSum==0){
lilac0112_1 16:4fadb7a87497 749 LineState = 0;
lilac0112_1 16:4fadb7a87497 750 }
lilac0112_1 16:4fadb7a87497 751 /*if(data.lnHold==7){
lilac0112_1 11:3efae754e6ef 752 if(data.lnRaw>0){
lilac0112_1 11:3efae754e6ef 753 //場外間際...減衰
lilac0112_1 11:3efae754e6ef 754 LineState = 3;
lilac0112_1 11:3efae754e6ef 755 }
lilac0112_1 11:3efae754e6ef 756 else{//data.lnRaw==0
lilac0112_1 11:3efae754e6ef 757 //場外...出力完全無効+反発
lilac0112_1 11:3efae754e6ef 758 //powerVoid & restore
lilac0112_1 11:3efae754e6ef 759 LineState = 4;
lilac0112_1 11:3efae754e6ef 760 }
lilac0112_1 11:3efae754e6ef 761 }
lilac0112_1 11:3efae754e6ef 762 else if(data.lnHold>0){
lilac0112_1 11:3efae754e6ef 763 if(data.lnRaw>0){
lilac0112_1 11:3efae754e6ef 764 //踏んでるけどまだ出てない...減衰
lilac0112_1 11:3efae754e6ef 765 LineState = 2;
lilac0112_1 11:3efae754e6ef 766 //timeout&reset...detach
lilac0112_1 11:3efae754e6ef 767 }
lilac0112_1 11:3efae754e6ef 768 else{//data.lnRaw==0
lilac0112_1 11:3efae754e6ef 769 //線をまたいでいるか，中途半端に線を踏んだあと復帰したか...減衰
lilac0112_1 11:3efae754e6ef 770 LineState = 1;
lilac0112_1 11:3efae754e6ef 771 //ping&reset
lilac0112_1 11:3efae754e6ef 772 //timeout&reset
lilac0112_1 11:3efae754e6ef 773 }
lilac0112_1 11:3efae754e6ef 774 }
lilac0112_1 11:3efae754e6ef 775 //else if(data.lnHold==0){...maxpower
lilac0112_1 13:b20921316f3c 776 if(data.FieldState==LINE_OUTSIDE){
lilac0112_1 13:b20921316f3c 777 LineState = 4;
lilac0112_1 16:4fadb7a87497 778 }*/
lilac0112_1 13:b20921316f3c 779
lilac0112_1 11:3efae754e6ef 780
lilac0112_1 14:b510adcb6065 781 if(pow_x>=0){
lilac0112_1 11:3efae754e6ef 782 if(data.ping[R_PING]<WhiteToWall[X_PING]) (*x)=LineDecline[LineState];
lilac0112_1 11:3efae754e6ef 783 else *x=1;
lilac0112_1 11:3efae754e6ef 784 }
lilac0112_1 11:3efae754e6ef 785 else{
lilac0112_1 11:3efae754e6ef 786 if(data.ping[L_PING]<WhiteToWall[X_PING]) (*x)=LineDecline[LineState];
lilac0112_1 11:3efae754e6ef 787 else *x=1;
lilac0112_1 11:3efae754e6ef 788 }
lilac0112_1 11:3efae754e6ef 789
lilac0112_1 14:b510adcb6065 790 if(pow_y>=0){
lilac0112_1 11:3efae754e6ef 791 if(data.ping[F_PING]<WhiteToWall[Y_PING]) (*y)=LineDecline[LineState];
lilac0112_1 11:3efae754e6ef 792 else *y=1;
lilac0112_1 11:3efae754e6ef 793 }
lilac0112_1 11:3efae754e6ef 794 else{
lilac0112_1 11:3efae754e6ef 795 if(data.ping[B_PING]<WhiteToWall[Y_PING]) (*y)=LineDecline[LineState];
lilac0112_1 11:3efae754e6ef 796 else *y=1;
lilac0112_1 11:3efae754e6ef 797 }
lilac0112_1 11:3efae754e6ef 798 }
lilac0112_1 13:b20921316f3c 799 void LineJudgeReturn(double pow_x, double pow_y, double *x, double *y){
lilac0112_1 12:bee8f883c33a 800 const int8_t static LineReturn[5] = {0, 0, 0, 0, 10};
lilac0112_1 12:bee8f883c33a 801 uint8_t LinePingState[4];
lilac0112_1 12:bee8f883c33a 802 //◎ボールを追う力とラインから離れる力の向きが違うならばラインから離れる力が優先される．
lilac0112_1 12:bee8f883c33a 803 //◎ボールを追う力とラインから離れる力の向きが同じならばボールを追う力が優先される．
lilac0112_1 12:bee8f883c33a 804 //◎ラインセンサ全てが場外になるまではボールを追う力は作用しない．ボールを追う力は場内に出るまで作用する．
lilac0112_1 12:bee8f883c33a 805 //×ラインのほぼ場外では常時ラインから離れる力が優先される．
lilac0112_1 12:bee8f883c33a 806 //※場外判定を行うには再び場内に戻る必要がある．
lilac0112_1 12:bee8f883c33a 807
lilac0112_1 12:bee8f883c33a 808 if(data.FieldState==LINE_OUTSIDE){
lilac0112_1 12:bee8f883c33a 809
lilac0112_1 12:bee8f883c33a 810 LinePingState[L_PING]=(data.ping[L_PING]<WhiteToWall[X_PING]);
lilac0112_1 12:bee8f883c33a 811 LinePingState[R_PING]=(data.ping[R_PING]<WhiteToWall[X_PING]);
lilac0112_1 12:bee8f883c33a 812 LinePingState[F_PING]=(data.ping[F_PING]<WhiteToWall[Y_PING]);
lilac0112_1 12:bee8f883c33a 813 LinePingState[B_PING]=(data.ping[B_PING]<WhiteToWall[Y_PING]);
lilac0112_1 12:bee8f883c33a 814
lilac0112_1 12:bee8f883c33a 815 //line間際の復帰力以外の力を作用させるか否か
lilac0112_1 12:bee8f883c33a 816 data.lnStop[X_LINE]=1;
lilac0112_1 12:bee8f883c33a 817 data.lnStop[Y_LINE]=1;
lilac0112_1 12:bee8f883c33a 818 //x
lilac0112_1 12:bee8f883c33a 819 if((LinePingState[L_PING]==0)&&(LinePingState[R_PING]==0)){
lilac0112_1 13:b20921316f3c 820 if(pow_x>0){
lilac0112_1 13:b20921316f3c 821 *x = -pow_x*(1-0.75);
lilac0112_1 13:b20921316f3c 822 }
lilac0112_1 13:b20921316f3c 823 else{
lilac0112_1 13:b20921316f3c 824 *x = pow_x*(1-0.75);
lilac0112_1 13:b20921316f3c 825 }
lilac0112_1 13:b20921316f3c 826 data.lnStop[X_LINE]=1;
lilac0112_1 12:bee8f883c33a 827 }
lilac0112_1 12:bee8f883c33a 828 if((LinePingState[L_PING]==0)&&(LinePingState[R_PING]==1)){
lilac0112_1 13:b20921316f3c 829 if(pow_x<0){
lilac0112_1 13:b20921316f3c 830 *x = 0;
lilac0112_1 13:b20921316f3c 831 data.lnStop[X_LINE]=1;
lilac0112_1 13:b20921316f3c 832 }
lilac0112_1 13:b20921316f3c 833 else{
lilac0112_1 13:b20921316f3c 834 *x = -LineReturn[4];
lilac0112_1 13:b20921316f3c 835 data.lnStop[X_LINE]=0;
lilac0112_1 13:b20921316f3c 836 }
lilac0112_1 12:bee8f883c33a 837 }
lilac0112_1 12:bee8f883c33a 838 if((LinePingState[L_PING]==1)&&(LinePingState[R_PING]==0)){
lilac0112_1 13:b20921316f3c 839 if(pow_x>0){
lilac0112_1 13:b20921316f3c 840 *x = 0;
lilac0112_1 13:b20921316f3c 841 data.lnStop[X_LINE]=1;
lilac0112_1 13:b20921316f3c 842 }
lilac0112_1 13:b20921316f3c 843 else{
lilac0112_1 13:b20921316f3c 844 *x = LineReturn[4];
lilac0112_1 13:b20921316f3c 845 data.lnStop[X_LINE]=0;
lilac0112_1 13:b20921316f3c 846 }
lilac0112_1 12:bee8f883c33a 847 }
lilac0112_1 12:bee8f883c33a 848 if((LinePingState[L_PING]==1)&&(LinePingState[R_PING]==1)){
lilac0112_1 12:bee8f883c33a 849 *x = 0;
lilac0112_1 13:b20921316f3c 850 data.lnStop[X_LINE]=0;
lilac0112_1 12:bee8f883c33a 851 }
lilac0112_1 12:bee8f883c33a 852
lilac0112_1 12:bee8f883c33a 853 //y
lilac0112_1 13:b20921316f3c 854 if((LinePingState[B_PING]==0)&&(LinePingState[F_PING]==0)){
lilac0112_1 13:b20921316f3c 855 if(pow_y>0){
lilac0112_1 13:b20921316f3c 856 *y = -pow_y*(1-0.75);
lilac0112_1 13:b20921316f3c 857 }
lilac0112_1 13:b20921316f3c 858 else{
lilac0112_1 13:b20921316f3c 859 *y = pow_y*(1-0.75);
lilac0112_1 13:b20921316f3c 860 }
lilac0112_1 13:b20921316f3c 861 data.lnStop[Y_LINE]=1;
lilac0112_1 12:bee8f883c33a 862 }
lilac0112_1 13:b20921316f3c 863 if((LinePingState[B_PING]==0)&&(LinePingState[F_PING]==1)){
lilac0112_1 13:b20921316f3c 864 if(pow_y<0){
lilac0112_1 13:b20921316f3c 865 *y = 0;
lilac0112_1 13:b20921316f3c 866 data.lnStop[Y_LINE]=1;
lilac0112_1 13:b20921316f3c 867 }
lilac0112_1 13:b20921316f3c 868 else{
lilac0112_1 13:b20921316f3c 869 *y = -LineReturn[4];
lilac0112_1 13:b20921316f3c 870 data.lnStop[Y_LINE]=0;
lilac0112_1 13:b20921316f3c 871 }
lilac0112_1 12:bee8f883c33a 872 }
lilac0112_1 13:b20921316f3c 873 if((LinePingState[B_PING]==1)&&(LinePingState[F_PING]==0)){
lilac0112_1 13:b20921316f3c 874 if(pow_y>0){
lilac0112_1 13:b20921316f3c 875 *y = 0;
lilac0112_1 13:b20921316f3c 876 data.lnStop[Y_LINE]=1;
lilac0112_1 13:b20921316f3c 877 }
lilac0112_1 13:b20921316f3c 878 else{
lilac0112_1 13:b20921316f3c 879 *y = LineReturn[4];
lilac0112_1 13:b20921316f3c 880 data.lnStop[Y_LINE]=0;
lilac0112_1 13:b20921316f3c 881 }
lilac0112_1 12:bee8f883c33a 882 }
lilac0112_1 13:b20921316f3c 883 if((LinePingState[B_PING]==1)&&(LinePingState[F_PING]==1)){
lilac0112_1 13:b20921316f3c 884 *y = 0;
lilac0112_1 13:b20921316f3c 885 data.lnStop[Y_LINE]=0;
lilac0112_1 12:bee8f883c33a 886 }
lilac0112_1 13:b20921316f3c 887
lilac0112_1 13:b20921316f3c 888
lilac0112_1 12:bee8f883c33a 889 }
lilac0112_1 12:bee8f883c33a 890 else{//data.FieldState==LINE_INSIDE
lilac0112_1 12:bee8f883c33a 891 *x = 0;
lilac0112_1 12:bee8f883c33a 892 *y = 0;
lilac0112_1 12:bee8f883c33a 893
lilac0112_1 12:bee8f883c33a 894 //line間際の復帰力以外の力を作用させるか否か
lilac0112_1 12:bee8f883c33a 895 data.lnStop[X_LINE]=1;
lilac0112_1 12:bee8f883c33a 896 data.lnStop[Y_LINE]=1;
lilac0112_1 12:bee8f883c33a 897 }
lilac0112_1 12:bee8f883c33a 898 }
lilac0112_1 12:bee8f883c33a 899 void LineJudgeReset(void){
lilac0112_1 14:b510adcb6065 900 if((data.lnRaw==0)&&(data.lnHold==7)){
lilac0112_1 16:4fadb7a87497 901 if(data.FieldSpot==LINE_INSIDE){
lilac0112_1 14:b510adcb6065 902 data.lnCorner[L_LINE] = (data.ping[L_PING]<OutToWall[X_PING]);
lilac0112_1 14:b510adcb6065 903 data.lnCorner[R_LINE] = (data.ping[R_PING]<OutToWall[X_PING]);
lilac0112_1 14:b510adcb6065 904 data.lnCorner[F_LINE] = (data.ping[F_PING]<OutToWall[Y_PING]);
lilac0112_1 14:b510adcb6065 905 data.lnCorner[B_LINE] = (data.ping[B_PING]<OutToWall[Y_PING]);
lilac0112_1 14:b510adcb6065 906 if(
lilac0112_1 14:b510adcb6065 907 (data.lnCorner[L_LINE])||
lilac0112_1 14:b510adcb6065 908 (data.lnCorner[R_LINE])||
lilac0112_1 14:b510adcb6065 909 (data.lnCorner[F_LINE])||
lilac0112_1 14:b510adcb6065 910 (data.lnCorner[B_LINE])
lilac0112_1 14:b510adcb6065 911 ){
lilac0112_1 16:4fadb7a87497 912 data.NonWall[L_LINE] = (data.ping[L_PING]>WhiteToWall[X_PING]);
lilac0112_1 16:4fadb7a87497 913 data.NonWall[R_LINE] = (data.ping[R_PING]>WhiteToWall[X_PING]);
lilac0112_1 16:4fadb7a87497 914 data.NonWall[F_LINE] = (data.ping[F_PING]>WhiteToWall[Y_PING]);
lilac0112_1 16:4fadb7a87497 915 data.NonWall[B_LINE] = (data.ping[B_PING]>WhiteToWall[Y_PING]);
lilac0112_1 16:4fadb7a87497 916 data.FieldSpot = LINE_OUTSIDE;
lilac0112_1 14:b510adcb6065 917 LineLiberate();
lilac0112_1 14:b510adcb6065 918 }
lilac0112_1 14:b510adcb6065 919 }
lilac0112_1 16:4fadb7a87497 920 else if(data.FieldSpot==LINE_OUTSIDE){
lilac0112_1 16:4fadb7a87497 921 data.FieldSpot = LINE_INSIDE;
lilac0112_1 16:4fadb7a87497 922 data.NonWall[L_LINE] = data.NonWall[R_LINE] = data.NonWall[F_LINE] = data.NonWall[B_LINE] = 0;
lilac0112_1 16:4fadb7a87497 923 LineLiberate();
lilac0112_1 16:4fadb7a87497 924 LineRankClear();
lilac0112_1 14:b510adcb6065 925 }
lilac0112_1 14:b510adcb6065 926 }
lilac0112_1 16:4fadb7a87497 927 if(data.FieldSpot == LINE_OUTSIDE){
lilac0112_1 16:4fadb7a87497 928 if(
lilac0112_1 16:4fadb7a87497 929 (
lilac0112_1 16:4fadb7a87497 930 (data.ping[L_PING]>=WhiteToWall[X_PING])||
lilac0112_1 16:4fadb7a87497 931 (data.NonWall[L_LINE]==1)||
lilac0112_1 16:4fadb7a87497 932 ((data.lnOrder[0]==A_SPOT)&&(data.lnOrder[1]==C_SPOT))
lilac0112_1 16:4fadb7a87497 933 //(data.lnOrder[0]==A_SPOT)
lilac0112_1 16:4fadb7a87497 934 )&&
lilac0112_1 16:4fadb7a87497 935 (
lilac0112_1 16:4fadb7a87497 936 (data.ping[R_PING]>=WhiteToWall[X_PING])||
lilac0112_1 16:4fadb7a87497 937 (data.NonWall[R_LINE]==1)||
lilac0112_1 16:4fadb7a87497 938 ((data.lnOrder[0]==B_SPOT)&&(data.lnOrder[1]==C_SPOT))
lilac0112_1 16:4fadb7a87497 939 //(data.lnOrder[0]==B_SPOT)
lilac0112_1 16:4fadb7a87497 940 )&&
lilac0112_1 16:4fadb7a87497 941 (
lilac0112_1 16:4fadb7a87497 942 (data.ping[F_PING]>=WhiteToWall[Y_PING])||
lilac0112_1 16:4fadb7a87497 943 (data.NonWall[F_LINE]==1)||
lilac0112_1 16:4fadb7a87497 944 ((data.lnOrder[0]==C_SPOT)&&(data.lnOrder[1]==A_SPOT))||
lilac0112_1 16:4fadb7a87497 945 ((data.lnOrder[0]==C_SPOT)&&(data.lnOrder[1]==B_SPOT))
lilac0112_1 16:4fadb7a87497 946 )&&
lilac0112_1 16:4fadb7a87497 947 ((data.ping[B_PING]>=WhiteToWall[Y_PING])||
lilac0112_1 16:4fadb7a87497 948 (data.NonWall[B_LINE]==1)||
lilac0112_1 16:4fadb7a87497 949 ((data.lnOrder[0]==A_SPOT)&&(data.lnOrder[1]==B_SPOT))||
lilac0112_1 16:4fadb7a87497 950 ((data.lnOrder[0]==B_SPOT)&&(data.lnOrder[1]==A_SPOT))
lilac0112_1 16:4fadb7a87497 951 )
lilac0112_1 16:4fadb7a87497 952 ){
lilac0112_1 16:4fadb7a87497 953 data.FieldSpot = LINE_INSIDE;
lilac0112_1 16:4fadb7a87497 954 data.NonWall[L_LINE] = data.NonWall[R_LINE] = data.NonWall[F_LINE] = data.NonWall[B_LINE] = 0;
lilac0112_1 16:4fadb7a87497 955 LineLiberate();
lilac0112_1 16:4fadb7a87497 956 LineRankClear();
lilac0112_1 13:b20921316f3c 957 }
lilac0112_1 16:4fadb7a87497 958 /*if(
lilac0112_1 16:4fadb7a87497 959 (data.ping[L_PING]>=WhiteToWall[X_PING])&&
lilac0112_1 16:4fadb7a87497 960 (data.ping[R_PING]>=WhiteToWall[X_PING])&&
lilac0112_1 16:4fadb7a87497 961 (data.ping[F_PING]>=WhiteToWall[Y_PING])&&
lilac0112_1 16:4fadb7a87497 962 (data.ping[B_PING]>=WhiteToWall[Y_PING])
lilac0112_1 16:4fadb7a87497 963 ){
lilac0112_1 16:4fadb7a87497 964 data.FieldSpot = LINE_INSIDE;
lilac0112_1 16:4fadb7a87497 965 LineLiberate();
lilac0112_1 16:4fadb7a87497 966 LineRankClear();
lilac0112_1 16:4fadb7a87497 967 }*/
lilac0112_1 16:4fadb7a87497 968 }
lilac0112_1 16:4fadb7a87497 969 if((data.FieldSpot == LINE_INSIDE)&&(0<data.lnHold)&&(data.lnHold<7)&&(data.lnRaw==0)){
lilac0112_1 16:4fadb7a87497 970 if(
lilac0112_1 16:4fadb7a87497 971 (
lilac0112_1 16:4fadb7a87497 972 (data.ping[L_PING]>=WhiteToWall[X_PING])||
lilac0112_1 16:4fadb7a87497 973 ((data.lnOrder[0]==A_SPOT)&&(data.lnOrder[1]==C_SPOT))
lilac0112_1 16:4fadb7a87497 974 //(data.lnOrder[0]==A_SPOT)
lilac0112_1 16:4fadb7a87497 975 )&&
lilac0112_1 16:4fadb7a87497 976 (
lilac0112_1 16:4fadb7a87497 977 (data.ping[R_PING]>=WhiteToWall[X_PING])||
lilac0112_1 16:4fadb7a87497 978 ((data.lnOrder[0]==B_SPOT)&&(data.lnOrder[1]==C_SPOT))
lilac0112_1 16:4fadb7a87497 979 //(data.lnOrder[0]==B_SPOT)
lilac0112_1 16:4fadb7a87497 980 )&&
lilac0112_1 16:4fadb7a87497 981 (
lilac0112_1 16:4fadb7a87497 982 (data.ping[F_PING]>=WhiteToWall[Y_PING])||
lilac0112_1 16:4fadb7a87497 983 ((data.lnOrder[0]==C_SPOT)&&(data.lnOrder[1]==A_SPOT))||
lilac0112_1 16:4fadb7a87497 984 ((data.lnOrder[0]==C_SPOT)&&(data.lnOrder[1]==B_SPOT))
lilac0112_1 16:4fadb7a87497 985 )&&
lilac0112_1 16:4fadb7a87497 986 ((data.ping[B_PING]>=WhiteToWall[Y_PING])||
lilac0112_1 16:4fadb7a87497 987 ((data.lnOrder[0]==A_SPOT)&&(data.lnOrder[1]==B_SPOT))||
lilac0112_1 16:4fadb7a87497 988 ((data.lnOrder[0]==B_SPOT)&&(data.lnOrder[1]==A_SPOT))
lilac0112_1 16:4fadb7a87497 989 )
lilac0112_1 16:4fadb7a87497 990 ){
lilac0112_1 16:4fadb7a87497 991 data.NonWall[L_LINE] = data.NonWall[R_LINE] = data.NonWall[F_LINE] = data.NonWall[B_LINE] = 0;
lilac0112_1 16:4fadb7a87497 992 LineLiberate();
lilac0112_1 13:b20921316f3c 993 }
lilac0112_1 13:b20921316f3c 994 }
lilac0112_1 12:bee8f883c33a 995 }
lilac0112_1 0:ea35c18c85fc 996 void modeAttack4(void){
lilac0112_1 10:6df631c39f9b 997 double ir_x_dir, ir_y_dir;
lilac0112_1 11:3efae754e6ef 998 double ir_x_turn, ir_y_turn;
lilac0112_1 11:3efae754e6ef 999 double ir_x, ir_y;
lilac0112_1 13:b20921316f3c 1000
lilac0112_1 14:b510adcb6065 1001 double LineSlowPower[2];
lilac0112_1 14:b510adcb6065 1002 double LineReturnPower[2];
lilac0112_1 14:b510adcb6065 1003
lilac0112_1 14:b510adcb6065 1004 //double pow_x, pow_y;
lilac0112_1 10:6df631c39f9b 1005 uint8_t ir_pow;
lilac0112_1 10:6df631c39f9b 1006 int vx,vy,vs;
lilac0112_1 15:17502a27a60b 1007 //static uint8_t data.FieldSpot;
lilac0112_1 11:3efae754e6ef 1008
lilac0112_1 11:3efae754e6ef 1009
lilac0112_1 10:6df631c39f9b 1010 if(sys.KickOffFlag==1){
lilac0112_1 10:6df631c39f9b 1011
lilac0112_1 14:b510adcb6065 1012 sys.IrBlind=1;
lilac0112_1 10:6df631c39f9b 1013 sys.LineBlind=0;
lilac0112_1 10:6df631c39f9b 1014 sys.PingBlind=0;
lilac0112_1 10:6df631c39f9b 1015
lilac0112_1 15:17502a27a60b 1016 data.FieldSpot = LINE_INSIDE;
lilac0112_1 14:b510adcb6065 1017 LineLiberate();
lilac0112_1 16:4fadb7a87497 1018 LineRankClear();
lilac0112_1 13:b20921316f3c 1019
lilac0112_1 10:6df631c39f9b 1020 sys.KickOffFlag=0;
lilac0112_1 0:ea35c18c85fc 1021 }
lilac0112_1 10:6df631c39f9b 1022 //data
lilac0112_1 10:6df631c39f9b 1023 if(sys.IrFlag==1){ReadIr();sys.IrFlag=0;}
lilac0112_1 10:6df631c39f9b 1024 if(sys.PidFlag==1){PidUpdate();sys.PidFlag=0;}
lilac0112_1 10:6df631c39f9b 1025 if(sys.UswFlag==1){ReadPing();sys.UswFlag=0;}
lilac0112_1 10:6df631c39f9b 1026 if(sys.UswFlag2==1){ReadPing2();sys.UswFlag2=0;}
lilac0112_1 10:6df631c39f9b 1027 //if(sys.KickFlag==1){DriveSolenoid();}
lilac0112_1 11:3efae754e6ef 1028 data.lnRaw = LineRaw;
lilac0112_1 11:3efae754e6ef 1029 data.lnHold = LineHold;
lilac0112_1 10:6df631c39f9b 1030
lilac0112_1 10:6df631c39f9b 1031 ir_x_dir = ir_move_val[0][data.irNotice][data.irPosition][IR_X_DIR];
lilac0112_1 10:6df631c39f9b 1032 ir_y_dir = ir_move_val[0][data.irNotice][data.irPosition][IR_Y_DIR];
lilac0112_1 10:6df631c39f9b 1033 ir_x_turn = ir_move_val[0][data.irNotice][data.irPosition][IR_X_TURN];
lilac0112_1 10:6df631c39f9b 1034 ir_y_turn = ir_move_val[0][data.irNotice][data.irPosition][IR_Y_TURN];
lilac0112_1 10:6df631c39f9b 1035 if(data.irNotice==IR_CLOSER){
lilac0112_1 10:6df631c39f9b 1036 ir_pow = sys.s_pow;
lilac0112_1 10:6df631c39f9b 1037 }
lilac0112_1 10:6df631c39f9b 1038 else if(data.irNotice==IR_CLOSE){
lilac0112_1 10:6df631c39f9b 1039 ir_pow = sys.m_pow;
lilac0112_1 10:6df631c39f9b 1040 }
lilac0112_1 10:6df631c39f9b 1041 else if(data.irNotice==IR_FAR){
lilac0112_1 10:6df631c39f9b 1042 ir_pow = sys.l_pow;
lilac0112_1 10:6df631c39f9b 1043 }
lilac0112_1 10:6df631c39f9b 1044 else{//data.irNotice==IR_NONE
lilac0112_1 10:6df631c39f9b 1045 ir_pow = 0;
lilac0112_1 10:6df631c39f9b 1046 }
lilac0112_1 10:6df631c39f9b 1047 if(sys.IrBlind==1) data.irNotice=IR_NONE;
lilac0112_1 10:6df631c39f9b 1048
lilac0112_1 11:3efae754e6ef 1049 ir_x = (ir_x_dir + ir_x_turn);
lilac0112_1 11:3efae754e6ef 1050 ir_y = (ir_y_dir + ir_y_turn);
lilac0112_1 11:3efae754e6ef 1051
lilac0112_1 14:b510adcb6065 1052 if(sys.LineBlind==1){
lilac0112_1 14:b510adcb6065 1053
lilac0112_1 14:b510adcb6065 1054 LineSlowPower[X_LINE] = 1.0;
lilac0112_1 14:b510adcb6065 1055 LineSlowPower[Y_LINE] = 1.0;
lilac0112_1 14:b510adcb6065 1056
lilac0112_1 14:b510adcb6065 1057 LineReturnPower[X_LINE] = 0.0;
lilac0112_1 14:b510adcb6065 1058 LineReturnPower[Y_LINE] = 0.0;
lilac0112_1 14:b510adcb6065 1059
lilac0112_1 14:b510adcb6065 1060 data.lnStop[X_LINE] = 1;
lilac0112_1 14:b510adcb6065 1061 data.lnStop[Y_LINE] = 1;
lilac0112_1 14:b510adcb6065 1062
lilac0112_1 15:17502a27a60b 1063 data.FieldSpot = LINE_INSIDE;
lilac0112_1 14:b510adcb6065 1064 }
lilac0112_1 14:b510adcb6065 1065 else{
lilac0112_1 16:4fadb7a87497 1066 LineJudgeReset();
lilac0112_1 16:4fadb7a87497 1067
lilac0112_1 14:b510adcb6065 1068 //LineJudgeSlow(ir_x, ir_y, &LineSlowPower[X_LINE], &LineSlowPower[Y_LINE]);
lilac0112_1 14:b510adcb6065 1069 //LineJudgeReturn(ir_x, ir_y, &LineReturnPower[X_LINE], &LineReturnPower[Y_LINE]);
lilac0112_1 14:b510adcb6065 1070
lilac0112_1 14:b510adcb6065 1071 LineSlowPower[X_LINE] = 1.0;
lilac0112_1 14:b510adcb6065 1072 LineSlowPower[Y_LINE] = 1.0;
lilac0112_1 14:b510adcb6065 1073
lilac0112_1 14:b510adcb6065 1074 LineReturnPower[X_LINE] = 0.0;
lilac0112_1 14:b510adcb6065 1075 LineReturnPower[Y_LINE] = 0.0;
lilac0112_1 14:b510adcb6065 1076
lilac0112_1 14:b510adcb6065 1077 data.lnStop[X_LINE] = 1;
lilac0112_1 14:b510adcb6065 1078 data.lnStop[Y_LINE] = 1;
lilac0112_1 14:b510adcb6065 1079
lilac0112_1 14:b510adcb6065 1080 }
lilac0112_1 15:17502a27a60b 1081 if(data.FieldSpot==LINE_OUTSIDE) LED = 0x9;
lilac0112_1 15:17502a27a60b 1082 if(data.FieldSpot==LINE_INSIDE) LED = 0x6;
lilac0112_1 14:b510adcb6065 1083
lilac0112_1 16:4fadb7a87497 1084 //LED = ((data.lnOrder[0]!=LINE_EMPTY)<<2) | ((data.lnOrder[1]!=LINE_EMPTY)<<1) | ((data.lnOrder[2]!=LINE_EMPTY)<<0);
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 }