CatPot 2015-2016 / Mbed 2 deprecated CatPot_Main_T_2v00

Main Program

Dependencies:   mbed AQM1602 HMC6352 PID

main_processing/strategy/strategy.cpp@42:02aaa806d929, 2016-02-04 (annotated)

Committer:
lilac0112_1
Date:
Thu Feb 04 20:45:41 2016 +0000
Revision:
42:02aaa806d929
Parent:
41:3cdcdec9c885
Child:
43:f11f68918299
InterruptForce are added and so on.

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 41:3cdcdec9c885 81 void LineClear_A(void){LineSign[A_SPOT]=0;data.lnFlag[A_SPOT]=0;}
lilac0112_1 41:3cdcdec9c885 82 void LineClear_B(void){LineSign[B_SPOT]=0;data.lnFlag[B_SPOT]=0;}
lilac0112_1 41:3cdcdec9c885 83 void LineClear_C(void){LineSign[C_SPOT]=0;data.lnFlag[C_SPOT]=0;}
lilac0112_1 40:f68474b1d5e7 84 void LineCall_A(void){
lilac0112_1 41:3cdcdec9c885 85 //
lilac0112_1 40:f68474b1d5e7 86 LineSign[A_SPOT] = 1;
lilac0112_1 40:f68474b1d5e7 87 if(LineSign[B_SPOT]==0){
lilac0112_1 40:f68474b1d5e7 88 LineFirst[X_AXIS] = A_SPOT;
lilac0112_1 40:f68474b1d5e7 89 }
lilac0112_1 40:f68474b1d5e7 90 if((LineSign[A_SPOT]==1)&&(LineSign[B_SPOT]==1)){
lilac0112_1 40:f68474b1d5e7 91 if(LineSign[C_SPOT]==0){
lilac0112_1 40:f68474b1d5e7 92 LineFirst[Y_AXIS] = AB_SPOT;
lilac0112_1 40:f68474b1d5e7 93 }
lilac0112_1 40:f68474b1d5e7 94 }
lilac0112_1 41:3cdcdec9c885 95 //
lilac0112_1 41:3cdcdec9c885 96 if((Line[A_SPOT].read()==1)||(1)) data.lnFlag[A_SPOT]=1;
lilac0112_1 41:3cdcdec9c885 97 //
lilac0112_1 42:02aaa806d929 98 Line_timeout[A_SPOT].attach(&LineClear_A, LINE_DELAY);
lilac0112_1 40:f68474b1d5e7 99 }
lilac0112_1 40:f68474b1d5e7 100 void LineCall_B(void){
lilac0112_1 41:3cdcdec9c885 101 //
lilac0112_1 40:f68474b1d5e7 102 LineSign[B_SPOT] = 1;
lilac0112_1 40:f68474b1d5e7 103 if(LineSign[A_SPOT]==0){
lilac0112_1 40:f68474b1d5e7 104 LineFirst[X_AXIS] = B_SPOT;
lilac0112_1 40:f68474b1d5e7 105 }
lilac0112_1 40:f68474b1d5e7 106 if((LineSign[A_SPOT]==1)&&(LineSign[B_SPOT]==1)){
lilac0112_1 40:f68474b1d5e7 107 if(LineSign[C_SPOT]==0){
lilac0112_1 40:f68474b1d5e7 108 LineFirst[Y_AXIS] = AB_SPOT;
lilac0112_1 40:f68474b1d5e7 109 }
lilac0112_1 40:f68474b1d5e7 110 }
lilac0112_1 41:3cdcdec9c885 111 //
lilac0112_1 41:3cdcdec9c885 112 if((Line[B_SPOT].read()==1)||(1)) data.lnFlag[B_SPOT]=1;
lilac0112_1 41:3cdcdec9c885 113 //
lilac0112_1 42:02aaa806d929 114 Line_timeout[B_SPOT].attach(&LineClear_B, LINE_DELAY);
lilac0112_1 40:f68474b1d5e7 115 }
lilac0112_1 40:f68474b1d5e7 116 void LineCall_C(void){
lilac0112_1 41:3cdcdec9c885 117 //
lilac0112_1 40:f68474b1d5e7 118 LineSign[C_SPOT] = 1;
lilac0112_1 40:f68474b1d5e7 119 if(!((LineSign[A_SPOT]==1)&&(LineSign[B_SPOT]==1))){
lilac0112_1 40:f68474b1d5e7 120 LineFirst[Y_AXIS] = C_SPOT;
lilac0112_1 40:f68474b1d5e7 121 }
lilac0112_1 41:3cdcdec9c885 122 //
lilac0112_1 41:3cdcdec9c885 123 if((Line[C_SPOT].read()==1)||(1)) data.lnFlag[C_SPOT]=1;
lilac0112_1 41:3cdcdec9c885 124 //
lilac0112_1 42:02aaa806d929 125 Line_timeout[C_SPOT].attach(&LineClear_C, LINE_DELAY);
lilac0112_1 40:f68474b1d5e7 126 }
lilac0112_1 28:8ac6c3c1e643 127 void modeAttack1(void){
lilac0112_1 31:0b0f64831771 128 double ir_x, ir_y;
lilac0112_1 31:0b0f64831771 129 int vx,vy,vs;
lilac0112_1 31:0b0f64831771 130 uint8_t LineDir[4];
lilac0112_1 31:0b0f64831771 131 uint8_t LineStop[2];
lilac0112_1 34:69bdf29a4442 132 uint8_t IrRange[4];
lilac0112_1 35:a0421268ee95 133 uint8_t LinePulse[4];
lilac0112_1 34:69bdf29a4442 134 uint8_t static LineBind[4];
lilac0112_1 34:69bdf29a4442 135 if(data.KickOffFlag==1){
lilac0112_1 34:69bdf29a4442 136 LineBind[0]=0;
lilac0112_1 34:69bdf29a4442 137 LineBind[1]=0;
lilac0112_1 34:69bdf29a4442 138 LineBind[2]=0;
lilac0112_1 34:69bdf29a4442 139 LineBind[3]=0;
lilac0112_1 35:a0421268ee95 140 LineReverseFlag=0;
lilac0112_1 40:f68474b1d5e7 141
lilac0112_1 40:f68474b1d5e7 142 LineSign[A_SPOT]=0;
lilac0112_1 40:f68474b1d5e7 143 LineSign[B_SPOT]=0;
lilac0112_1 40:f68474b1d5e7 144 LineSign[C_SPOT]=0;
lilac0112_1 40:f68474b1d5e7 145
lilac0112_1 41:3cdcdec9c885 146 data.lnFlag[A_SPOT]=0;
lilac0112_1 41:3cdcdec9c885 147 data.lnFlag[B_SPOT]=0;
lilac0112_1 41:3cdcdec9c885 148 data.lnFlag[C_SPOT]=0;
lilac0112_1 41:3cdcdec9c885 149
lilac0112_1 34:69bdf29a4442 150 data.KickOffFlag=0;
lilac0112_1 34:69bdf29a4442 151 }
lilac0112_1 31:0b0f64831771 152 if(data.IrFlag==1){
lilac0112_1 31:0b0f64831771 153 ReadIr();
lilac0112_1 31:0b0f64831771 154 data.IrFlag=0;
lilac0112_1 31:0b0f64831771 155 }
lilac0112_1 31:0b0f64831771 156 if(data.PidFlag==1){
lilac0112_1 41:3cdcdec9c885 157 //Line_ticker.detach();
lilac0112_1 31:0b0f64831771 158 PidUpdate();
lilac0112_1 41:3cdcdec9c885 159 //Line_ticker.attach(&ReadLine, 0.005);
lilac0112_1 31:0b0f64831771 160 data.PidFlag=0;
lilac0112_1 31:0b0f64831771 161 }
lilac0112_1 31:0b0f64831771 162 ir_x = ir_move_val[data.irNotice][data.irPosition][IR_X];
lilac0112_1 31:0b0f64831771 163 ir_y = ir_move_val[data.irNotice][data.irPosition][IR_Y];
lilac0112_1 31:0b0f64831771 164 if(data.irPosition<8){
lilac0112_1 31:0b0f64831771 165 ir_x *= data.l_pow;
lilac0112_1 31:0b0f64831771 166 ir_y *= data.l_pow;
lilac0112_1 31:0b0f64831771 167 }
lilac0112_1 31:0b0f64831771 168 else{
lilac0112_1 31:0b0f64831771 169 ir_x *= data.s_pow;
lilac0112_1 31:0b0f64831771 170 ir_y *= data.s_pow;
lilac0112_1 31:0b0f64831771 171 }
lilac0112_1 31:0b0f64831771 172
lilac0112_1 31:0b0f64831771 173 //Lineを考慮していないIrのみの値
lilac0112_1 31:0b0f64831771 174 vx = ir_x;
lilac0112_1 31:0b0f64831771 175 vy = ir_y;
lilac0112_1 31:0b0f64831771 176 //Line検出方向を調べる
lilac0112_1 35:a0421268ee95 177 LineDir[A_SPOT] = (!((vx>0)&&((data.lnFlag[A_SPOT]==1)&&(data.lnFlag[B_SPOT]==0))&&(1)));
lilac0112_1 35:a0421268ee95 178 LineDir[B_SPOT] = (!((vx<0)&&((data.lnFlag[A_SPOT]==0)&&(data.lnFlag[B_SPOT]==1))&&(1)));
lilac0112_1 35:a0421268ee95 179 LineDir[C_SPOT] = (!((vy<0)&&((data.lnFlag[C_SPOT]==1)&&(1 ))&&(1)));
lilac0112_1 35:a0421268ee95 180 LineDir[AB_SPOT] = (!((vy>0)&&((data.lnFlag[A_SPOT]==1)&&(data.lnFlag[B_SPOT]==1))&&(1)));
lilac0112_1 31:0b0f64831771 181
lilac0112_1 31:0b0f64831771 182 LineStop[X_AXIS] = LineDir[A_SPOT]*LineDir[B_SPOT];
lilac0112_1 31:0b0f64831771 183 LineStop[Y_AXIS] = LineDir[C_SPOT]*LineDir[AB_SPOT];
lilac0112_1 31:0b0f64831771 184
lilac0112_1 34:69bdf29a4442 185 //Ir
lilac0112_1 36:50092d488f78 186 //strict
lilac0112_1 36:50092d488f78 187 /*
lilac0112_1 34:69bdf29a4442 188 IrRange[A_SPOT] = ((18<=data.irPosition)&&(data.irPosition<=19))||((8<=data.irPosition)&&(data.irPosition<=10))
lilac0112_1 34:69bdf29a4442 189 ||((6<=data.irPosition)&&(data.irPosition<=7))||(( 1)&&(data.irPosition<=1));
lilac0112_1 34:69bdf29a4442 190 IrRange[B_SPOT] = ((12<=data.irPosition)&&(data.irPosition<=16))||((2<=data.irPosition)&&(data.irPosition<=5));
lilac0112_1 34:69bdf29a4442 191 IrRange[C_SPOT] = ((15<=data.irPosition)&&(data.irPosition<=19))||((4<=data.irPosition)&&(data.irPosition<=7));
lilac0112_1 34:69bdf29a4442 192 IrRange[AB_SPOT] = ((9<=data.irPosition)&&(data.irPosition<=13))||(( 1)&&(data.irPosition<=3));
lilac0112_1 36:50092d488f78 193 */
lilac0112_1 36:50092d488f78 194 //sweet
lilac0112_1 36:50092d488f78 195 IrRange[A_SPOT] = ((19<=data.irPosition)&&(data.irPosition<=19))||((8<=data.irPosition)&&(data.irPosition<=9))
lilac0112_1 36:50092d488f78 196 ||((6<=data.irPosition)&&(data.irPosition<=7))||(( 1)&&(data.irPosition<=1));
lilac0112_1 36:50092d488f78 197 IrRange[B_SPOT] = ((13<=data.irPosition)&&(data.irPosition<=15))||((2<=data.irPosition)&&(data.irPosition<=5));
lilac0112_1 36:50092d488f78 198 IrRange[C_SPOT] = ((16<=data.irPosition)&&(data.irPosition<=18))||((4<=data.irPosition)&&(data.irPosition<=7));
lilac0112_1 36:50092d488f78 199 IrRange[AB_SPOT] = ((10<=data.irPosition)&&(data.irPosition<=12))||(( 1)&&(data.irPosition<=3));
lilac0112_1 34:69bdf29a4442 200
lilac0112_1 35:a0421268ee95 201
lilac0112_1 35:a0421268ee95 202 LinePulse[A_SPOT] = ((IrRange[A_SPOT]==1)&&(LineDir[A_SPOT]==0));
lilac0112_1 35:a0421268ee95 203 LinePulse[B_SPOT] = ((IrRange[B_SPOT]==1)&&(LineDir[B_SPOT]==0));
lilac0112_1 35:a0421268ee95 204 LinePulse[C_SPOT] = ((IrRange[C_SPOT]==1)&&(LineDir[C_SPOT]==0));
lilac0112_1 35:a0421268ee95 205 LinePulse[AB_SPOT] = ((IrRange[AB_SPOT]==1)&&(LineDir[AB_SPOT]==0));
lilac0112_1 35:a0421268ee95 206
lilac0112_1 35:a0421268ee95 207 LineBind[A_SPOT] = ((LinePulse[A_SPOT])||((IrRange[A_SPOT]==1)&&(LineBind[A_SPOT]==1)))&&(LineBind[B_SPOT]==0);
lilac0112_1 35:a0421268ee95 208 LineBind[B_SPOT] = ((LinePulse[B_SPOT])||((IrRange[B_SPOT]==1)&&(LineBind[B_SPOT]==1)))&&(LineBind[A_SPOT]==0);
lilac0112_1 35:a0421268ee95 209 LineBind[C_SPOT] = ((LinePulse[C_SPOT])||((IrRange[C_SPOT]==1)&&(LineBind[C_SPOT]==1)))&&(LineBind[AB_SPOT]==0);
lilac0112_1 35:a0421268ee95 210 LineBind[AB_SPOT] = ((LinePulse[AB_SPOT])||((IrRange[AB_SPOT]==1)&&(LineBind[AB_SPOT]==1)))&&(LineBind[C_SPOT]==0);
lilac0112_1 35:a0421268ee95 211 /*
lilac0112_1 34:69bdf29a4442 212 LineBind[A_SPOT] = ((IrRange[A_SPOT]==1)&&((LineDir[A_SPOT]==0)||(LineBind[A_SPOT]==1)));
lilac0112_1 34:69bdf29a4442 213 LineBind[B_SPOT] = ((IrRange[B_SPOT]==1)&&((LineDir[B_SPOT]==0)||(LineBind[B_SPOT]==1)));
lilac0112_1 34:69bdf29a4442 214 LineBind[C_SPOT] = ((IrRange[C_SPOT]==1)&&((LineDir[C_SPOT]==0)||(LineBind[C_SPOT]==1)));
lilac0112_1 34:69bdf29a4442 215 LineBind[AB_SPOT] = ((IrRange[AB_SPOT]==1)&&((LineDir[AB_SPOT]==0)||(LineBind[AB_SPOT]==1)));
lilac0112_1 35:a0421268ee95 216 */
lilac0112_1 37:ae5661306900 217 vx = vx*LineStop[X_AXIS] + (LINE_RF)*(vy!=0)*((-1)*(LineDir[A_SPOT]==0) + (LineDir[B_SPOT]==0));
lilac0112_1 37:ae5661306900 218 vy = vy*LineStop[Y_AXIS] + (LINE_RF)*(vx!=0)*((LineDir[C_SPOT]==0) + (-1)*(LineDir[AB_SPOT]==0));
lilac0112_1 37:ae5661306900 219 vs = data.OutputPID;
lilac0112_1 34:69bdf29a4442 220 if((LineBind[A_SPOT]==1)||(LineBind[B_SPOT]==1)||(LineBind[C_SPOT]==1)||(LineBind[AB_SPOT]==1)){
lilac0112_1 37:ae5661306900 221 if(RawLineSignal>0){
lilac0112_1 37:ae5661306900 222 vx=(LINE_RF*2)*((-1)*IrRange[A_SPOT] + IrRange[B_SPOT]);
lilac0112_1 37:ae5661306900 223 vy=(LINE_RF*2)*( IrRange[C_SPOT] + (-1)*IrRange[AB_SPOT]);
lilac0112_1 40:f68474b1d5e7 224 /*vx=(LINE_RF*2)*((-1)*(LineFirst[X_AXIS] == A_SPOT) + (LineFirst[X_AXIS] == B_SPOT));
lilac0112_1 40:f68474b1d5e7 225 vy=(LINE_RF*2)*( (LineFirst[Y_AXIS] == C_SPOT) + (-1)*(LineFirst[Y_AXIS] == AB_SPOT));
lilac0112_1 42:02aaa806d929 226 Line_timeout[A_SPOT].attach(&LineClear_A, LINE_DELAY);
lilac0112_1 42:02aaa806d929 227 Line_timeout[B_SPOT].attach(&LineClear_B, LINE_DELAY);
lilac0112_1 42:02aaa806d929 228 Line_timeout[C_SPOT].attach(&LineClear_C, LINE_DELAY);*/
lilac0112_1 35:a0421268ee95 229 }
lilac0112_1 35:a0421268ee95 230 else{
lilac0112_1 35:a0421268ee95 231 vx=0;
lilac0112_1 35:a0421268ee95 232 vy=0;
lilac0112_1 35:a0421268ee95 233 }
lilac0112_1 31:0b0f64831771 234 }
lilac0112_1 41:3cdcdec9c885 235 if(RawLineSignal>0){
lilac0112_1 42:02aaa806d929 236 Line_timeout[A_SPOT].attach(&LineClear_A, LINE_DELAY);
lilac0112_1 42:02aaa806d929 237 Line_timeout[B_SPOT].attach(&LineClear_B, LINE_DELAY);
lilac0112_1 42:02aaa806d929 238 Line_timeout[C_SPOT].attach(&LineClear_C, LINE_DELAY);
lilac0112_1 41:3cdcdec9c885 239 }
lilac0112_1 31:0b0f64831771 240 move(
lilac0112_1 37:ae5661306900 241 vx,
lilac0112_1 37:ae5661306900 242 vy,
lilac0112_1 31:0b0f64831771 243 vs
lilac0112_1 31:0b0f64831771 244 );
lilac0112_1 31:0b0f64831771 245 if(data.MotorFlag==1){
lilac0112_1 31:0b0f64831771 246 tx_motor();
lilac0112_1 31:0b0f64831771 247 data.MotorFlag=0;
lilac0112_1 31:0b0f64831771 248 }
lilac0112_1 28:8ac6c3c1e643 249 return;
lilac0112_1 28:8ac6c3c1e643 250 }
lilac0112_1 28:8ac6c3c1e643 251 void modeAttack2(void){
lilac0112_1 42:02aaa806d929 252 double ir_x, ir_y;
lilac0112_1 42:02aaa806d929 253 int vx,vy,vs, LineForce[2];
lilac0112_1 42:02aaa806d929 254 uint8_t LineDir[4];
lilac0112_1 42:02aaa806d929 255 uint8_t LineOn[4];
lilac0112_1 42:02aaa806d929 256 uint8_t LineStop[2];
lilac0112_1 42:02aaa806d929 257 uint8_t IrRange[4];
lilac0112_1 42:02aaa806d929 258 uint8_t static LineBind[4];
lilac0112_1 42:02aaa806d929 259 if(data.KickOffFlag==1){
lilac0112_1 42:02aaa806d929 260 LineBind[0]=0;
lilac0112_1 42:02aaa806d929 261 LineBind[1]=0;
lilac0112_1 42:02aaa806d929 262 LineBind[2]=0;
lilac0112_1 42:02aaa806d929 263 LineBind[3]=0;
lilac0112_1 42:02aaa806d929 264 LineReverseFlag=0;
lilac0112_1 42:02aaa806d929 265
lilac0112_1 42:02aaa806d929 266 LineSign[A_SPOT]=0;
lilac0112_1 42:02aaa806d929 267 LineSign[B_SPOT]=0;
lilac0112_1 42:02aaa806d929 268 LineSign[C_SPOT]=0;
lilac0112_1 42:02aaa806d929 269
lilac0112_1 42:02aaa806d929 270 data.lnFlag[A_SPOT]=0;
lilac0112_1 42:02aaa806d929 271 data.lnFlag[B_SPOT]=0;
lilac0112_1 42:02aaa806d929 272 data.lnFlag[C_SPOT]=0;
lilac0112_1 42:02aaa806d929 273
lilac0112_1 42:02aaa806d929 274 HmcResetFlag = 0;
lilac0112_1 42:02aaa806d929 275 PingFlag = 0;
lilac0112_1 42:02aaa806d929 276
lilac0112_1 42:02aaa806d929 277 data.KickOffFlag=0;
lilac0112_1 42:02aaa806d929 278 }
lilac0112_1 42:02aaa806d929 279 if(data.IrFlag==1){
lilac0112_1 42:02aaa806d929 280 ReadIr();
lilac0112_1 42:02aaa806d929 281 data.IrFlag=0;
lilac0112_1 42:02aaa806d929 282 }
lilac0112_1 42:02aaa806d929 283 if(data.PidFlag==1){
lilac0112_1 42:02aaa806d929 284 PidUpdate();
lilac0112_1 42:02aaa806d929 285 data.PidFlag=0;
lilac0112_1 42:02aaa806d929 286 }
lilac0112_1 42:02aaa806d929 287 /*
lilac0112_1 42:02aaa806d929 288 if(PingFlag==1){
lilac0112_1 42:02aaa806d929 289 ReadPing();
lilac0112_1 42:02aaa806d929 290 PingFlag=0;
lilac0112_1 42:02aaa806d929 291 }
lilac0112_1 42:02aaa806d929 292 if(HmcResetFlag==1){
lilac0112_1 42:02aaa806d929 293 HmcReset();
lilac0112_1 42:02aaa806d929 294 HmcResetFlag=0;
lilac0112_1 42:02aaa806d929 295 }
lilac0112_1 42:02aaa806d929 296 */
lilac0112_1 42:02aaa806d929 297 ir_x = ir_move_val[data.irNotice][data.irPosition][IR_X];
lilac0112_1 42:02aaa806d929 298 ir_y = ir_move_val[data.irNotice][data.irPosition][IR_Y];
lilac0112_1 42:02aaa806d929 299 if(data.irPosition<8){
lilac0112_1 42:02aaa806d929 300 ir_x *= data.l_pow;
lilac0112_1 42:02aaa806d929 301 ir_y *= data.l_pow;
lilac0112_1 42:02aaa806d929 302 }
lilac0112_1 42:02aaa806d929 303 else{
lilac0112_1 42:02aaa806d929 304 ir_x *= data.s_pow;
lilac0112_1 42:02aaa806d929 305 ir_y *= data.s_pow;
lilac0112_1 42:02aaa806d929 306 }
lilac0112_1 42:02aaa806d929 307
lilac0112_1 42:02aaa806d929 308 //Lineを考慮していないIrのみの値
lilac0112_1 42:02aaa806d929 309 vx = ir_x;
lilac0112_1 42:02aaa806d929 310 vy = ir_y;
lilac0112_1 42:02aaa806d929 311
lilac0112_1 42:02aaa806d929 312 if((data.irPosition==11)&&(vy>0)){
lilac0112_1 42:02aaa806d929 313 vy += 10;//前進加速
lilac0112_1 42:02aaa806d929 314 }
lilac0112_1 42:02aaa806d929 315 if((data.irPosition==17)&&(data.motorlog[X_AXIS]>0)){
lilac0112_1 42:02aaa806d929 316 vx *= -1.0;//背後回り込みの左右判断
lilac0112_1 42:02aaa806d929 317 }
lilac0112_1 42:02aaa806d929 318 /*
lilac0112_1 42:02aaa806d929 319 if((data.irPosition==17)&&(data.ping[L_PING]>data.ping[R_PING])){
lilac0112_1 42:02aaa806d929 320 vx *= -1.0;//背後回り込みの左右判断
lilac0112_1 42:02aaa806d929 321 }
lilac0112_1 42:02aaa806d929 322 */
lilac0112_1 42:02aaa806d929 323 //Lineを踏み始めた方向を調べる
lilac0112_1 42:02aaa806d929 324 LineDir[A_SPOT] = (vx>0)&&((data.lnFlag[A_SPOT]==1)&&(1 ))&&(LineFirst[X_AXIS] == A_SPOT);
lilac0112_1 42:02aaa806d929 325 LineDir[B_SPOT] = (vx<0)&&((data.lnFlag[B_SPOT]==1)&&(1 ))&&(LineFirst[X_AXIS] == B_SPOT);
lilac0112_1 42:02aaa806d929 326 LineDir[C_SPOT] = (vy<0)&&((data.lnFlag[C_SPOT]==1)&&(1 ))&&(LineFirst[Y_AXIS] == C_SPOT);
lilac0112_1 42:02aaa806d929 327 LineDir[AB_SPOT]= (vy>0)&&((data.lnFlag[A_SPOT]==1)&&(data.lnFlag[B_SPOT]==1))&&(LineFirst[Y_AXIS] == AB_SPOT);
lilac0112_1 42:02aaa806d929 328
lilac0112_1 42:02aaa806d929 329 //Irボールの方向
lilac0112_1 42:02aaa806d929 330 //strict
lilac0112_1 42:02aaa806d929 331 /*
lilac0112_1 42:02aaa806d929 332 IrRange[A_SPOT] = ((18<=data.irPosition)&&(data.irPosition<=19))||((8<=data.irPosition)&&(data.irPosition<=10))
lilac0112_1 42:02aaa806d929 333 ||((6<=data.irPosition)&&(data.irPosition<=7))||(( 1)&&(data.irPosition<=1));
lilac0112_1 42:02aaa806d929 334 IrRange[B_SPOT] = ((12<=data.irPosition)&&(data.irPosition<=16))||((2<=data.irPosition)&&(data.irPosition<=5));
lilac0112_1 42:02aaa806d929 335 IrRange[C_SPOT] = ((15<=data.irPosition)&&(data.irPosition<=19))||((4<=data.irPosition)&&(data.irPosition<=7));
lilac0112_1 42:02aaa806d929 336 IrRange[AB_SPOT] = ((9<=data.irPosition)&&(data.irPosition<=13))||(( 1)&&(data.irPosition<=3));
lilac0112_1 42:02aaa806d929 337 */
lilac0112_1 42:02aaa806d929 338 //sweet
lilac0112_1 42:02aaa806d929 339 IrRange[A_SPOT] = ((19<=data.irPosition)&&(data.irPosition<=19))||((8<=data.irPosition)&&(data.irPosition<=9))
lilac0112_1 42:02aaa806d929 340 ||((6<=data.irPosition)&&(data.irPosition<=7))||(( 1)&&(data.irPosition<=1));
lilac0112_1 42:02aaa806d929 341 IrRange[B_SPOT] = ((13<=data.irPosition)&&(data.irPosition<=15))||((2<=data.irPosition)&&(data.irPosition<=5));
lilac0112_1 42:02aaa806d929 342 IrRange[C_SPOT] = ((16<=data.irPosition)&&(data.irPosition<=18))||((4<=data.irPosition)&&(data.irPosition<=7));
lilac0112_1 42:02aaa806d929 343 IrRange[AB_SPOT] = ((10<=data.irPosition)&&(data.irPosition<=12))||(( 1)&&(data.irPosition<=3));
lilac0112_1 42:02aaa806d929 344 //白線を踏み始めた方向とボールの方向が一致．(SelfHold)
lilac0112_1 42:02aaa806d929 345 LineBind[A_SPOT] = (IrRange[A_SPOT]==1)&&((LineDir[A_SPOT]==1)||(LineBind[A_SPOT]==1));
lilac0112_1 42:02aaa806d929 346 LineBind[B_SPOT] = (IrRange[B_SPOT]==1)&&((LineDir[B_SPOT]==1)||(LineBind[B_SPOT]==1));
lilac0112_1 42:02aaa806d929 347 LineBind[C_SPOT] = (IrRange[C_SPOT]==1)&&((LineDir[C_SPOT]==1)||(LineBind[C_SPOT]==1));
lilac0112_1 42:02aaa806d929 348 LineBind[AB_SPOT] = (IrRange[AB_SPOT]==1)&&((LineDir[AB_SPOT]==1)||(LineBind[AB_SPOT]==1));
lilac0112_1 42:02aaa806d929 349
lilac0112_1 42:02aaa806d929 350 LineStop[X_AXIS] = (LineBind[A_SPOT]==0)*(LineBind[B_SPOT]==0);
lilac0112_1 42:02aaa806d929 351 LineStop[Y_AXIS] = (LineBind[C_SPOT]==0)*(LineBind[AB_SPOT]==0);
lilac0112_1 42:02aaa806d929 352
lilac0112_1 42:02aaa806d929 353 //白線踏んでる
lilac0112_1 42:02aaa806d929 354 if(RawLineSignal>0){
lilac0112_1 42:02aaa806d929 355 LineOn[A_SPOT] = (LineSign[A_SPOT]==1) &&(LineFirst[X_AXIS]==A_SPOT);
lilac0112_1 42:02aaa806d929 356 LineOn[B_SPOT] = (LineSign[B_SPOT]==1) &&(LineFirst[X_AXIS]==B_SPOT);
lilac0112_1 42:02aaa806d929 357 LineOn[C_SPOT] = (LineSign[C_SPOT]==1) &&(LineFirst[Y_AXIS]==C_SPOT);
lilac0112_1 42:02aaa806d929 358 LineOn[AB_SPOT] = ((LineSign[A_SPOT]==1)&&(LineSign[B_SPOT]==1))&&(LineFirst[Y_AXIS]==AB_SPOT);
lilac0112_1 42:02aaa806d929 359 //外側に向かう力を消す．
lilac0112_1 42:02aaa806d929 360 //x
lilac0112_1 42:02aaa806d929 361 if(((LineOn[A_SPOT]==1)&&(vx>0))||((LineOn[B_SPOT]==1)&&(vx<0))){
lilac0112_1 42:02aaa806d929 362 vx=0;
lilac0112_1 42:02aaa806d929 363 //yの力を加える．
lilac0112_1 42:02aaa806d929 364 if(vy>0){vy += 10;}
lilac0112_1 42:02aaa806d929 365 if(vy<0){vy -= 10;}
lilac0112_1 42:02aaa806d929 366 }
lilac0112_1 42:02aaa806d929 367 //y
lilac0112_1 42:02aaa806d929 368 if(((LineOn[C_SPOT]==1)&&(vy<0))||((LineOn[AB_SPOT]==1)&&(vy>0))){
lilac0112_1 42:02aaa806d929 369 vy=0;
lilac0112_1 42:02aaa806d929 370 }
lilac0112_1 42:02aaa806d929 371 //内側に向かう力を加える．
lilac0112_1 42:02aaa806d929 372 LineForce[X_AXIS] = (LINE_RF*2)*((-1)*(LineOn[A_SPOT]==1) + ( 1)*(LineOn[B_SPOT]==1));
lilac0112_1 42:02aaa806d929 373 LineForce[Y_AXIS] = (LINE_RF*2)*(( 1)*(LineOn[C_SPOT]==1) + (-1)*(LineOn[AB_SPOT]==1));
lilac0112_1 42:02aaa806d929 374 Line_timeout[A_SPOT].attach(&LineClear_A, LINE_DELAY);
lilac0112_1 42:02aaa806d929 375 Line_timeout[B_SPOT].attach(&LineClear_B, LINE_DELAY);
lilac0112_1 42:02aaa806d929 376 Line_timeout[C_SPOT].attach(&LineClear_C, LINE_DELAY);
lilac0112_1 42:02aaa806d929 377 }
lilac0112_1 42:02aaa806d929 378 else{
lilac0112_1 42:02aaa806d929 379 LineForce[X_AXIS] = 0;
lilac0112_1 42:02aaa806d929 380 LineForce[Y_AXIS] = 0;
lilac0112_1 42:02aaa806d929 381 }
lilac0112_1 42:02aaa806d929 382
lilac0112_1 42:02aaa806d929 383 vx = vx*LineStop[X_AXIS] + LineForce[X_AXIS];
lilac0112_1 42:02aaa806d929 384 vy = vy*LineStop[Y_AXIS] + LineForce[Y_AXIS];
lilac0112_1 42:02aaa806d929 385 vs = data.OutputPID;
lilac0112_1 42:02aaa806d929 386 move(
lilac0112_1 42:02aaa806d929 387 vx,
lilac0112_1 42:02aaa806d929 388 vy,
lilac0112_1 42:02aaa806d929 389 vs
lilac0112_1 42:02aaa806d929 390 );
lilac0112_1 42:02aaa806d929 391 data.motorlog[X_AXIS] += vx;
lilac0112_1 42:02aaa806d929 392 data.motorlog[Y_AXIS] += vy;
lilac0112_1 42:02aaa806d929 393 if(data.MotorFlag==1){
lilac0112_1 42:02aaa806d929 394 tx_motor();
lilac0112_1 42:02aaa806d929 395 data.MotorFlag=0;
lilac0112_1 42:02aaa806d929 396 }
lilac0112_1 42:02aaa806d929 397 return;
lilac0112_1 42:02aaa806d929 398 }
lilac0112_1 42:02aaa806d929 399 void modeAttack3(void){
lilac0112_1 42:02aaa806d929 400 if(data.KickFlag==1){
lilac0112_1 42:02aaa806d929 401 DriveSolenoid();
lilac0112_1 42:02aaa806d929 402 }
lilac0112_1 42:02aaa806d929 403 return;
lilac0112_1 42:02aaa806d929 404 }
lilac0112_1 42:02aaa806d929 405 void modeAttack4(void){
lilac0112_1 42:02aaa806d929 406 return;
lilac0112_1 42:02aaa806d929 407 }
lilac0112_1 42:02aaa806d929 408 void modeAttack5(void){
lilac0112_1 34:69bdf29a4442 409 if(data.IrFlag==1){
lilac0112_1 34:69bdf29a4442 410 ReadIr();
lilac0112_1 34:69bdf29a4442 411 data.IrFlag=0;
lilac0112_1 34:69bdf29a4442 412 }
lilac0112_1 34:69bdf29a4442 413 if(data.PidFlag==1){
lilac0112_1 34:69bdf29a4442 414 PidUpdate();
lilac0112_1 34:69bdf29a4442 415 data.PidFlag=0;
lilac0112_1 34:69bdf29a4442 416 }
lilac0112_1 34:69bdf29a4442 417 move(0,0,10);
lilac0112_1 34:69bdf29a4442 418 if(data.MotorFlag==1){
lilac0112_1 34:69bdf29a4442 419 tx_motor();
lilac0112_1 34:69bdf29a4442 420 data.MotorFlag=0;
lilac0112_1 34:69bdf29a4442 421 }
lilac0112_1 34:69bdf29a4442 422
lilac0112_1 28:8ac6c3c1e643 423 return;
lilac0112_1 28:8ac6c3c1e643 424 }