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@2:635947de1583, 2016-03-03 (annotated)

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