CatPot 2015-2016 / Mbed 2 deprecated CatPot_Main_T_2v00

Main Program

Dependencies:   mbed AQM1602 HMC6352 PID

main_processing/strategy/strategy.cpp@45:c23f25c00d0d, 2016-02-25 (annotated)

Committer:
lilac0112_1
Date:
Thu Feb 25 06:12:47 2016 +0000
Revision:
45:c23f25c00d0d
Parent:
43:f11f68918299 
Hokushinetsu(20:1)

Who changed what in which revision?

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